From a9f771c5647214637a5845e02e741511ffc8c606 Mon Sep 17 00:00:00 2001 From: Stefan Scherzinger Date: Mon, 5 Aug 2024 17:44:03 +0200 Subject: [PATCH 01/25] Add tests for the dummy's motion profile They belong to #b22fa1d --- .../tests/test_motion_profile.py | 129 ++++++++++++++++++ 1 file changed, 129 insertions(+) create mode 100644 schunk_egu_egk_gripper_dummy/tests/test_motion_profile.py diff --git a/schunk_egu_egk_gripper_dummy/tests/test_motion_profile.py b/schunk_egu_egk_gripper_dummy/tests/test_motion_profile.py new file mode 100644 index 0000000..120df57 --- /dev/null +++ b/schunk_egu_egk_gripper_dummy/tests/test_motion_profile.py @@ -0,0 +1,129 @@ +from src.dummy import LinearMotion +import pytest +import time + + +def test_motion_initializes_with_initial_state_and_target_state(): + initial_pos = 0.0 + initial_speed = 0.0 + target_pos = 0.1 + target_speed = 55.1 + motion = LinearMotion( + initial_pos=initial_pos, + initial_speed=initial_speed, + target_pos=target_pos, + target_speed=target_speed, + ) + assert pytest.approx(motion.initial_pos) == initial_pos + assert pytest.approx(motion.initial_speed) == initial_speed + assert pytest.approx(motion.target_pos) == target_pos + assert pytest.approx(motion.target_speed) == target_speed + + +def test_motion_corrects_negative_or_zero_speed_arguments(): + initial_pos = 0.0 + initial_speed = -0.3 + target_pos = 0.1 + target_speed = 0.0 + motion = LinearMotion( + initial_pos=initial_pos, + initial_speed=initial_speed, + target_pos=target_pos, + target_speed=target_speed, + ) + assert pytest.approx(motion.target_speed) == motion.min_speed + assert pytest.approx(motion.initial_speed) == 0.0 + + target_speed = -0.123 + motion = LinearMotion( + initial_pos=initial_pos, + initial_speed=initial_speed, + target_pos=target_pos, + target_speed=target_speed, + ) + assert pytest.approx(motion.target_speed) == motion.min_speed + + +def test_motion_samples_current_state_at_given_time(): + initial_pos = 0.0 + initial_speed = 0.0 + target_pos = 1.3 + target_speed = 55.1 + motion = LinearMotion( + initial_pos=initial_pos, + initial_speed=initial_speed, + target_pos=target_pos, + target_speed=target_speed, + ) + start = time.time() + time.sleep(0.01) + now = time.time() - start + current_pos, current_speed = motion.sample(t=now) + assert isinstance(current_pos, float) + assert isinstance(current_speed, float) + assert current_pos >= initial_pos + assert current_pos <= target_pos + assert current_speed >= initial_speed + assert current_speed <= target_speed + + +def test_motion_samples_initial_state_at_time_less_or_equal_zero(): + initial_pos = 0.0 + initial_speed = 0.0 + target_pos = 0.1 + target_speed = 55.1 + motion = LinearMotion( + initial_pos=initial_pos, + initial_speed=initial_speed, + target_pos=target_pos, + target_speed=target_speed, + ) + current_pos, current_speed = motion.sample(t=-0.001) + assert pytest.approx(current_pos) == motion.initial_pos + assert pytest.approx(current_speed) == motion.initial_speed + + +def test_motion_samples_target_position_at_time_after_finish(): + initial_pos = 0.0 + initial_speed = 0.0 + target_pos = 0.1 + target_speed = 55.1 + motion = LinearMotion( + initial_pos=initial_pos, + initial_speed=initial_speed, + target_pos=target_pos, + target_speed=target_speed, + ) + current_pos, current_speed = motion.sample(t=1000) + assert pytest.approx(current_pos) == motion.target_pos + assert pytest.approx(current_speed) == 0.0 + + +def test_motion_supports_both_directions(): + initial_pos = 5.0 + initial_speed = 0.0 + target_speed = 5.678 + + # Positive direction + target_pos = 12.34 + motion = LinearMotion( + initial_pos=initial_pos, + initial_speed=initial_speed, + target_pos=target_pos, + target_speed=target_speed, + ) + current_pos_1, _ = motion.sample(t=0.01) + current_pos_2, _ = motion.sample(t=0.02) + assert current_pos_1 < current_pos_2 + + # Negative direction + target_pos = 1.234 + motion = LinearMotion( + initial_pos=initial_pos, + initial_speed=initial_speed, + target_pos=target_pos, + target_speed=target_speed, + ) + current_pos_1, _ = motion.sample(t=0.01) + current_pos_2, _ = motion.sample(t=0.02) + assert current_pos_1 > current_pos_2 From 73f2014fd6f77d631468290f01de4f43af3fd003 Mon Sep 17 00:00:00 2001 From: Stefan Scherzinger Date: Tue, 6 Aug 2024 08:55:14 +0200 Subject: [PATCH 02/25] Add a test for the driver's topic publishing Also use `MultiThreadedExecutor` for the subscribing nodes. The normal `spin()` lead to `ValueError: generator already executing in ROS2 framework` errors when having them in the same test case. --- schunk_egu_egk_gripper_tests/test/helpers.py | 13 +++++++++---- .../test/test_ros_interfaces.py | 18 ++++++++++++++++++ 2 files changed, 27 insertions(+), 4 deletions(-) diff --git a/schunk_egu_egk_gripper_tests/test/helpers.py b/schunk_egu_egk_gripper_tests/test/helpers.py index 966c404..7f1fea1 100644 --- a/schunk_egu_egk_gripper_tests/test/helpers.py +++ b/schunk_egu_egk_gripper_tests/test/helpers.py @@ -1,21 +1,26 @@ from threading import Thread, Event -import rclpy from rclpy.node import Node from typing import Any from schunk_egu_egk_gripper_interfaces.msg import State # type: ignore[attr-defined] import uuid +from rclpy.executors import MultiThreadedExecutor class TopicGetsPublished(Node): def __init__(self, topic: str, type: Any): - node_name = "check_topic" + str(uuid.uuid4()).replace("-", "") - super().__init__(node_name) + self.node_name = "check_topic" + str(uuid.uuid4()).replace("-", "") + super().__init__(self.node_name) self.event = Event() self.data = None self.sub = self.create_subscription(type, topic, self.msg_cb, 3) - self.thread = Thread(target=lambda node: rclpy.spin(node), args=(self,)) + self.thread = Thread(target=self.spin) self.thread.start() + def spin(self) -> None: + executor = MultiThreadedExecutor() + executor.add_node(self) + executor.spin() + def msg_cb(self, data: Any) -> None: self.data = data self.event.set() diff --git a/schunk_egu_egk_gripper_tests/test/test_ros_interfaces.py b/schunk_egu_egk_gripper_tests/test/test_ros_interfaces.py index 48651fe..8a64b04 100644 --- a/schunk_egu_egk_gripper_tests/test/test_ros_interfaces.py +++ b/schunk_egu_egk_gripper_tests/test/test_ros_interfaces.py @@ -3,6 +3,10 @@ from rclpy.node import Node import time from test.conftest import launch_description +from test.helpers import TopicGetsPublished +from sensor_msgs.msg import JointState +from schunk_egu_egk_gripper_interfaces.msg import State # type: ignore[attr-defined] +from diagnostic_msgs.msg import DiagnosticArray def check_each_in(elements: list, node_method: str) -> None: @@ -57,3 +61,17 @@ def test_driver_advertices_all_relevant_actions( ] action_list = [a + "/_action/status" for a in action_list] check_each_in(action_list, "get_topic_names_and_types") + + +@pytest.mark.launch(fixture=launch_description) +def test_driver_publishes_on_all_advertised_topics( + launch_context, isolated, gripper_dummy +): + timeout = 3 + joint_states = TopicGetsPublished("/joint_states", JointState) + state = TopicGetsPublished("/state", State) + diagnostics = TopicGetsPublished("/diagnostics", DiagnosticArray) + + assert joint_states.event.wait(timeout) + assert state.event.wait(timeout) + assert diagnostics.event.wait(timeout) From 7623e29dcd610f0f654b27d2f9eec6d037dc32b9 Mon Sep 17 00:00:00 2001 From: Stefan Scherzinger Date: Tue, 6 Aug 2024 19:03:55 +0200 Subject: [PATCH 03/25] Use a helper method for calling services This should reduce boilerplate code when testing each service interface. --- schunk_egu_egk_gripper_tests/test/helpers.py | 21 +++++++++++++++++++ .../test/test_functionality.py | 13 +++++------- 2 files changed, 26 insertions(+), 8 deletions(-) diff --git a/schunk_egu_egk_gripper_tests/test/helpers.py b/schunk_egu_egk_gripper_tests/test/helpers.py index 7f1fea1..2f015cd 100644 --- a/schunk_egu_egk_gripper_tests/test/helpers.py +++ b/schunk_egu_egk_gripper_tests/test/helpers.py @@ -30,3 +30,24 @@ def get_current_state(variable: str): topic = TopicGetsPublished("/state", State) topic.event.wait() return getattr(topic.data, variable) + + +class ServiceReturnsResult(Node): + def __init__(self, service: str, type: Any, msg: Any): + self.node_name = "call_service" + str(uuid.uuid4()).replace("-", "") + super().__init__(self.node_name) + self.event = Event() + srv = self.create_client(type, service) + while not srv.wait_for_service(1.0): + ... + self.future = srv.call_async(msg) + self.result = None + self.thread = Thread(target=self.spin) + self.thread.start() + + def spin(self) -> None: + executor = MultiThreadedExecutor() + executor.add_node(self) + executor.spin_until_future_complete(self.future) + self.result = self.future.result() + self.event.set() diff --git a/schunk_egu_egk_gripper_tests/test/test_functionality.py b/schunk_egu_egk_gripper_tests/test/test_functionality.py index e0b9d5f..8703344 100644 --- a/schunk_egu_egk_gripper_tests/test/test_functionality.py +++ b/schunk_egu_egk_gripper_tests/test/test_functionality.py @@ -9,7 +9,7 @@ from schunk_egu_egk_gripper_interfaces.srv import ( # type: ignore[attr-defined] Acknowledge, ) -from test.helpers import get_current_state +from test.helpers import get_current_state, ServiceReturnsResult @pytest.mark.launch(fixture=launch_description) @@ -19,13 +19,10 @@ def test_driver_starts_in_ready_state(launch_context, isolated, gripper_dummy): @pytest.mark.launch(fixture=launch_description) def test_driver_is_ready_after_acknowledge(launch_context, isolated, gripper_dummy): - node = Node("test") - activate_srv = node.create_client(Acknowledge, "/acknowledge") - while not activate_srv.wait_for_service(1.0): - pass - future = activate_srv.call_async(Acknowledge.Request()) - rclpy.spin_until_future_complete(node, future) - assert future.result().success is True + timeout = 3 + service = ServiceReturnsResult("/acknowledge", Acknowledge, Acknowledge.Request()) + service.event.wait(timeout) + assert service.result.success is True @pytest.mark.launch(fixture=launch_description) From f9bb926771f5e40924751a3a6e1445a49e4c1f7d Mon Sep 17 00:00:00 2001 From: Stefan Scherzinger Date: Wed, 7 Aug 2024 07:54:19 +0200 Subject: [PATCH 04/25] Recompose the driver's test files We now have a cleaner separation between topics, services, and actions. --- schunk_egu_egk_gripper_tests/test/helpers.py | 11 +++ ...{test_functionality.py => test_actions.py} | 32 ++++---- .../test/test_http_interface.py | 14 ---- .../test/test_ros_interfaces.py | 77 ------------------- .../test/test_services.py | 33 ++++++++ .../test/test_startup.py | 10 +++ .../test/test_topics.py | 13 ++++ 7 files changed, 84 insertions(+), 106 deletions(-) rename schunk_egu_egk_gripper_tests/test/{test_functionality.py => test_actions.py} (60%) delete mode 100644 schunk_egu_egk_gripper_tests/test/test_http_interface.py delete mode 100644 schunk_egu_egk_gripper_tests/test/test_ros_interfaces.py create mode 100644 schunk_egu_egk_gripper_tests/test/test_services.py create mode 100644 schunk_egu_egk_gripper_tests/test/test_startup.py create mode 100644 schunk_egu_egk_gripper_tests/test/test_topics.py diff --git a/schunk_egu_egk_gripper_tests/test/helpers.py b/schunk_egu_egk_gripper_tests/test/helpers.py index 2f015cd..69e5123 100644 --- a/schunk_egu_egk_gripper_tests/test/helpers.py +++ b/schunk_egu_egk_gripper_tests/test/helpers.py @@ -4,6 +4,7 @@ from schunk_egu_egk_gripper_interfaces.msg import State # type: ignore[attr-defined] import uuid from rclpy.executors import MultiThreadedExecutor +import time class TopicGetsPublished(Node): @@ -51,3 +52,13 @@ def spin(self) -> None: executor.spin_until_future_complete(self.future) self.result = self.future.result() self.event.set() + + +def check_each_in(elements: list, node_method: str) -> None: + node = Node("test") + until_ready = 2.0 # sec + time.sleep(until_ready) + existing = getattr(node, node_method)() + advertised = [i[0] for i in existing] + for element in elements: + assert element in advertised diff --git a/schunk_egu_egk_gripper_tests/test/test_functionality.py b/schunk_egu_egk_gripper_tests/test/test_actions.py similarity index 60% rename from schunk_egu_egk_gripper_tests/test/test_functionality.py rename to schunk_egu_egk_gripper_tests/test/test_actions.py index 8703344..d741d82 100644 --- a/schunk_egu_egk_gripper_tests/test/test_functionality.py +++ b/schunk_egu_egk_gripper_tests/test/test_actions.py @@ -1,33 +1,35 @@ import pytest +from test.conftest import launch_description +from test.helpers import check_each_in import rclpy +import time from rclpy.node import Node -from test.conftest import launch_description from rclpy.action import ActionClient from schunk_egu_egk_gripper_interfaces.action import ( # type: ignore[attr-defined] MoveToAbsolutePosition, ) -from schunk_egu_egk_gripper_interfaces.srv import ( # type: ignore[attr-defined] - Acknowledge, -) -from test.helpers import get_current_state, ServiceReturnsResult - - -@pytest.mark.launch(fixture=launch_description) -def test_driver_starts_in_ready_state(launch_context, isolated, gripper_dummy): - assert get_current_state(variable="ready_for_operation") is True @pytest.mark.launch(fixture=launch_description) -def test_driver_is_ready_after_acknowledge(launch_context, isolated, gripper_dummy): - timeout = 3 - service = ServiceReturnsResult("/acknowledge", Acknowledge, Acknowledge.Request()) - service.event.wait(timeout) - assert service.result.success is True +def test_driver_advertices_all_relevant_actions( + launch_context, isolated, gripper_dummy +): + action_list = [ + "/grip", + "/grip_with_position", + "/gripper_control", + "/move_to_absolute_position", + "/move_to_relative_position", + "/release_workpiece", + ] + action_list = [a + "/_action/status" for a in action_list] + check_each_in(action_list, "get_topic_names_and_types") @pytest.mark.launch(fixture=launch_description) def test_driver_moves_to_absolute_position(launch_context, isolated, gripper_dummy): node = Node("move_test") + time.sleep(2) client = ActionClient(node, MoveToAbsolutePosition, "/move_to_absolute_position") client.wait_for_server() diff --git a/schunk_egu_egk_gripper_tests/test/test_http_interface.py b/schunk_egu_egk_gripper_tests/test/test_http_interface.py deleted file mode 100644 index 7bf76ca..0000000 --- a/schunk_egu_egk_gripper_tests/test/test_http_interface.py +++ /dev/null @@ -1,14 +0,0 @@ -import pytest -from test.conftest import launch_description -import time -from sensor_msgs.msg import JointState -from test.helpers import TopicGetsPublished - - -@pytest.mark.launch(fixture=launch_description) -def test_driver_connnects_to_gripper_dummy(launch_context, isolated, gripper_dummy): - until_dummy_ready = 3 - timeout = 3 - - time.sleep(until_dummy_ready) - assert TopicGetsPublished("/joint_states", JointState).event.wait(timeout) diff --git a/schunk_egu_egk_gripper_tests/test/test_ros_interfaces.py b/schunk_egu_egk_gripper_tests/test/test_ros_interfaces.py deleted file mode 100644 index 8a64b04..0000000 --- a/schunk_egu_egk_gripper_tests/test/test_ros_interfaces.py +++ /dev/null @@ -1,77 +0,0 @@ -#!/usr/bin/env python3 -import pytest -from rclpy.node import Node -import time -from test.conftest import launch_description -from test.helpers import TopicGetsPublished -from sensor_msgs.msg import JointState -from schunk_egu_egk_gripper_interfaces.msg import State # type: ignore[attr-defined] -from diagnostic_msgs.msg import DiagnosticArray - - -def check_each_in(elements: list, node_method: str) -> None: - node = Node("test") - until_ready = 2.0 # sec - time.sleep(until_ready) - existing = getattr(node, node_method)() - advertised = [i[0] for i in existing] - for element in elements: - assert element in advertised - - -@pytest.mark.launch(fixture=launch_description) -def test_driver_advertices_all_relevant_topics(launch_context, isolated, gripper_dummy): - topic_list = [ - "/diagnostics", - "/joint_states", - "/state", - ] - check_each_in(topic_list, "get_topic_names_and_types") - - -@pytest.mark.launch(fixture=launch_description) -def test_driver_advertices_all_relevant_services( - launch_context, isolated, gripper_dummy -): - service_list = [ - "/acknowledge", - "/brake_test", - "/fast_stop", - "/gripper_info", - "/prepare_for_shutdown", - "/reconnect", - "/release_for_manual_movement", - "/softreset", - "/stop", - ] - check_each_in(service_list, "get_service_names_and_types") - - -@pytest.mark.launch(fixture=launch_description) -def test_driver_advertices_all_relevant_actions( - launch_context, isolated, gripper_dummy -): - action_list = [ - "/grip", - "/grip_with_position", - "/gripper_control", - "/move_to_absolute_position", - "/move_to_relative_position", - "/release_workpiece", - ] - action_list = [a + "/_action/status" for a in action_list] - check_each_in(action_list, "get_topic_names_and_types") - - -@pytest.mark.launch(fixture=launch_description) -def test_driver_publishes_on_all_advertised_topics( - launch_context, isolated, gripper_dummy -): - timeout = 3 - joint_states = TopicGetsPublished("/joint_states", JointState) - state = TopicGetsPublished("/state", State) - diagnostics = TopicGetsPublished("/diagnostics", DiagnosticArray) - - assert joint_states.event.wait(timeout) - assert state.event.wait(timeout) - assert diagnostics.event.wait(timeout) diff --git a/schunk_egu_egk_gripper_tests/test/test_services.py b/schunk_egu_egk_gripper_tests/test/test_services.py new file mode 100644 index 0000000..a0b9062 --- /dev/null +++ b/schunk_egu_egk_gripper_tests/test/test_services.py @@ -0,0 +1,33 @@ +import pytest +from test.conftest import launch_description +from test.helpers import check_each_in +from schunk_egu_egk_gripper_interfaces.srv import ( # type: ignore[attr-defined] + Acknowledge, +) +from test.helpers import ServiceReturnsResult + + +@pytest.mark.launch(fixture=launch_description) +def test_driver_advertices_all_relevant_services( + launch_context, isolated, gripper_dummy +): + service_list = [ + "/acknowledge", + "/brake_test", + "/fast_stop", + "/gripper_info", + "/prepare_for_shutdown", + "/reconnect", + "/release_for_manual_movement", + "/softreset", + "/stop", + ] + check_each_in(service_list, "get_service_names_and_types") + + +@pytest.mark.launch(fixture=launch_description) +def test_driver_is_ready_after_acknowledge(launch_context, isolated, gripper_dummy): + timeout = 3 + service = ServiceReturnsResult("/acknowledge", Acknowledge, Acknowledge.Request()) + service.event.wait(timeout) + assert service.result.success is True diff --git a/schunk_egu_egk_gripper_tests/test/test_startup.py b/schunk_egu_egk_gripper_tests/test/test_startup.py new file mode 100644 index 0000000..4f5c14a --- /dev/null +++ b/schunk_egu_egk_gripper_tests/test/test_startup.py @@ -0,0 +1,10 @@ +import pytest +from test.conftest import launch_description +from test.helpers import get_current_state +import time + + +@pytest.mark.launch(fixture=launch_description) +def test_driver_starts_in_ready_state(launch_context, isolated, gripper_dummy): + time.sleep(2) + assert get_current_state(variable="ready_for_operation") is True diff --git a/schunk_egu_egk_gripper_tests/test/test_topics.py b/schunk_egu_egk_gripper_tests/test/test_topics.py new file mode 100644 index 0000000..dffb61d --- /dev/null +++ b/schunk_egu_egk_gripper_tests/test/test_topics.py @@ -0,0 +1,13 @@ +import pytest +from test.conftest import launch_description +from test.helpers import check_each_in + + +@pytest.mark.launch(fixture=launch_description) +def test_driver_advertices_all_relevant_topics(launch_context, isolated, gripper_dummy): + topic_list = [ + "/diagnostics", + "/joint_states", + "/state", + ] + check_each_in(topic_list, "get_topic_names_and_types") From ac04bfe20b3134cb004805ced521030fc1720a09 Mon Sep 17 00:00:00 2001 From: Stefan Scherzinger Date: Thu, 8 Aug 2024 10:07:14 +0200 Subject: [PATCH 05/25] Use pytest's `module` scope for tests Also use a single fixture `running_driver` for testing topics, services, and actions. This avoids brittle sleeps and makes sure that everything is up and running before starting the actual tests. The `test_startup.py` became obsolete with this. --- schunk_egu_egk_gripper_tests/test/conftest.py | 18 +++++++++++++++--- schunk_egu_egk_gripper_tests/test/helpers.py | 3 +-- .../test/test_actions.py | 8 ++------ .../test/test_launch.py | 2 +- .../test/test_services.py | 9 +++------ .../test/test_startup.py | 10 ---------- .../test/test_topics.py | 2 +- 7 files changed, 23 insertions(+), 29 deletions(-) delete mode 100644 schunk_egu_egk_gripper_tests/test/test_startup.py diff --git a/schunk_egu_egk_gripper_tests/test/conftest.py b/schunk_egu_egk_gripper_tests/test/conftest.py index cd37991..4c35d3d 100644 --- a/schunk_egu_egk_gripper_tests/test/conftest.py +++ b/schunk_egu_egk_gripper_tests/test/conftest.py @@ -7,6 +7,8 @@ import launch_pytest import subprocess from ament_index_python.packages import get_package_share_directory +from test.helpers import get_current_state +import time # We avoid black's F811, F401 linting warnings @@ -15,14 +17,14 @@ # https://docs.pytest.org/en/7.1.x/reference/fixtures.html#conftest-py-sharing-fixtures-across-multiple-files # noqa: E501 -@pytest.fixture() +@pytest.fixture(scope="module") def isolated(): rclpy.init() yield rclpy.shutdown() -@pytest.fixture() +@pytest.fixture(scope="module") def gripper_dummy(): package_name = "schunk_egu_egk_gripper_dummy" dummy_dir = get_package_share_directory(package_name) @@ -37,7 +39,7 @@ def gripper_dummy(): print("Stopped gripper dummy") -@launch_pytest.fixture +@launch_pytest.fixture(scope="module") def launch_description(): setup = IncludeLaunchDescription( PathJoinSubstitution( @@ -53,3 +55,13 @@ def launch_description(): }.items(), ) return LaunchDescription([setup, launch_pytest.actions.ReadyToTest()]) + + +@pytest.fixture(scope="module") +def running_driver(gripper_dummy, isolated, launch_context): + + print("Waiting until the driver is connected") + while not get_current_state(variable="ready_for_operation") is True: + time.sleep(0.1) + yield + ... diff --git a/schunk_egu_egk_gripper_tests/test/helpers.py b/schunk_egu_egk_gripper_tests/test/helpers.py index 69e5123..8096a8b 100644 --- a/schunk_egu_egk_gripper_tests/test/helpers.py +++ b/schunk_egu_egk_gripper_tests/test/helpers.py @@ -39,8 +39,7 @@ def __init__(self, service: str, type: Any, msg: Any): super().__init__(self.node_name) self.event = Event() srv = self.create_client(type, service) - while not srv.wait_for_service(1.0): - ... + srv.wait_for_service(1.0) self.future = srv.call_async(msg) self.result = None self.thread = Thread(target=self.spin) diff --git a/schunk_egu_egk_gripper_tests/test/test_actions.py b/schunk_egu_egk_gripper_tests/test/test_actions.py index d741d82..8c8a82f 100644 --- a/schunk_egu_egk_gripper_tests/test/test_actions.py +++ b/schunk_egu_egk_gripper_tests/test/test_actions.py @@ -2,7 +2,6 @@ from test.conftest import launch_description from test.helpers import check_each_in import rclpy -import time from rclpy.node import Node from rclpy.action import ActionClient from schunk_egu_egk_gripper_interfaces.action import ( # type: ignore[attr-defined] @@ -11,9 +10,7 @@ @pytest.mark.launch(fixture=launch_description) -def test_driver_advertices_all_relevant_actions( - launch_context, isolated, gripper_dummy -): +def test_driver_advertices_all_relevant_actions(running_driver): action_list = [ "/grip", "/grip_with_position", @@ -27,9 +24,8 @@ def test_driver_advertices_all_relevant_actions( @pytest.mark.launch(fixture=launch_description) -def test_driver_moves_to_absolute_position(launch_context, isolated, gripper_dummy): +def test_driver_moves_to_absolute_position(running_driver): node = Node("move_test") - time.sleep(2) client = ActionClient(node, MoveToAbsolutePosition, "/move_to_absolute_position") client.wait_for_server() diff --git a/schunk_egu_egk_gripper_tests/test/test_launch.py b/schunk_egu_egk_gripper_tests/test/test_launch.py index 3b85f46..6b86c54 100644 --- a/schunk_egu_egk_gripper_tests/test/test_launch.py +++ b/schunk_egu_egk_gripper_tests/test/test_launch.py @@ -6,7 +6,7 @@ @pytest.mark.launch(fixture=launch_description) -def test_normal_startup_works(launch_context, isolated, gripper_dummy): +def test_normal_startup_works(launch_context, isolated): node = Node("test_startup") until_ready = 2.0 # sec time.sleep(until_ready) diff --git a/schunk_egu_egk_gripper_tests/test/test_services.py b/schunk_egu_egk_gripper_tests/test/test_services.py index a0b9062..b169c5a 100644 --- a/schunk_egu_egk_gripper_tests/test/test_services.py +++ b/schunk_egu_egk_gripper_tests/test/test_services.py @@ -8,9 +8,7 @@ @pytest.mark.launch(fixture=launch_description) -def test_driver_advertices_all_relevant_services( - launch_context, isolated, gripper_dummy -): +def test_driver_advertices_all_relevant_services(running_driver): service_list = [ "/acknowledge", "/brake_test", @@ -26,8 +24,7 @@ def test_driver_advertices_all_relevant_services( @pytest.mark.launch(fixture=launch_description) -def test_driver_is_ready_after_acknowledge(launch_context, isolated, gripper_dummy): - timeout = 3 +def test_driver_is_ready_after_acknowledge(running_driver): service = ServiceReturnsResult("/acknowledge", Acknowledge, Acknowledge.Request()) - service.event.wait(timeout) + service.event.wait(timeout=1) assert service.result.success is True diff --git a/schunk_egu_egk_gripper_tests/test/test_startup.py b/schunk_egu_egk_gripper_tests/test/test_startup.py deleted file mode 100644 index 4f5c14a..0000000 --- a/schunk_egu_egk_gripper_tests/test/test_startup.py +++ /dev/null @@ -1,10 +0,0 @@ -import pytest -from test.conftest import launch_description -from test.helpers import get_current_state -import time - - -@pytest.mark.launch(fixture=launch_description) -def test_driver_starts_in_ready_state(launch_context, isolated, gripper_dummy): - time.sleep(2) - assert get_current_state(variable="ready_for_operation") is True diff --git a/schunk_egu_egk_gripper_tests/test/test_topics.py b/schunk_egu_egk_gripper_tests/test/test_topics.py index dffb61d..315e5a0 100644 --- a/schunk_egu_egk_gripper_tests/test/test_topics.py +++ b/schunk_egu_egk_gripper_tests/test/test_topics.py @@ -4,7 +4,7 @@ @pytest.mark.launch(fixture=launch_description) -def test_driver_advertices_all_relevant_topics(launch_context, isolated, gripper_dummy): +def test_driver_advertices_all_relevant_topics(running_driver): topic_list = [ "/diagnostics", "/joint_states", From 767676c9db1e6289f5b692b2ad2c67ee9714ecea Mon Sep 17 00:00:00 2001 From: Stefan Scherzinger Date: Thu, 8 Aug 2024 15:25:02 +0200 Subject: [PATCH 06/25] Add a test for the gripper's _brake test_ service --- schunk_egu_egk_gripper_dummy/src/dummy.py | 7 +++- .../tests/test_dummy.py | 38 ++++++++++++++++--- .../test/test_services.py | 8 ++++ 3 files changed, 46 insertions(+), 7 deletions(-) diff --git a/schunk_egu_egk_gripper_dummy/src/dummy.py b/schunk_egu_egk_gripper_dummy/src/dummy.py index 2d7a045..7e4e978 100644 --- a/schunk_egu_egk_gripper_dummy/src/dummy.py +++ b/schunk_egu_egk_gripper_dummy/src/dummy.py @@ -246,6 +246,8 @@ def get_actual_speed(self) -> float: return struct.unpack("f", bytes.fromhex(read_speed))[0] def process_control_bits(self) -> None: + # Command received toggle + self.toggle_status_bit(bit=5) # Acknowledge if self.get_control_bit(2) == 1: @@ -254,9 +256,12 @@ def process_control_bits(self) -> None: self.set_status_error("00") self.set_status_diagnostics("00") + # Brake test + if self.get_control_bit(30) == 1: + self.set_status_bit(bit=4, value=True) + # Move to absolute position if self.get_control_bit(13) == 1: - self.toggle_status_bit(5) self.move( target_pos=self.get_target_position(), target_speed=self.get_target_speed(), diff --git a/schunk_egu_egk_gripper_dummy/tests/test_dummy.py b/schunk_egu_egk_gripper_dummy/tests/test_dummy.py index b8a610a..58f62c7 100644 --- a/schunk_egu_egk_gripper_dummy/tests/test_dummy.py +++ b/schunk_egu_egk_gripper_dummy/tests/test_dummy.py @@ -44,7 +44,7 @@ def test_dummy_starts_in_error_state(): def test_dummy_is_ready_after_acknowledge(): dummy = Dummy() - control_double_word = "04000000" + control_double_word = "04000000" # bit 2 set_position = "00000000" set_speed = "00000000" gripping_force = "00000000" @@ -59,12 +59,27 @@ def test_dummy_is_ready_after_acknowledge(): assert dummy.get_status_diagnostics() == "0" +def test_dummy_always_toggles_command_received_bit(): + dummy = Dummy() + before = dummy.get_status_bit(bit=5) # command received toggle + control_double_word = "00000000" + set_position = "00000000" + set_speed = "00000000" + gripping_force = "00000000" + command = { + "inst": dummy.plc_output, + "value": control_double_word + set_position + set_speed + gripping_force, + } + dummy.post(command) + after = dummy.get_status_bit(bit=5) + assert after != before + + def test_dummy_moves_to_absolute_position(): dummy = Dummy() target_pos = 12.345 target_speed = 50.3 - - control_double_word = "00200000" + control_double_word = "00200000" # bit 13 set_position = bytes(struct.pack("f", target_pos)).hex().upper() set_speed = bytes(struct.pack("f", target_speed)).hex().upper() gripping_force = "00000000" @@ -72,14 +87,25 @@ def test_dummy_moves_to_absolute_position(): "inst": dummy.plc_output, "value": control_double_word + set_position + set_speed + gripping_force, } - before = dummy.get_status_bit(bit=5) # command received toggle # Motion dummy.post(command) # Done assert pytest.approx(dummy.get_actual_position()) == target_pos - after = dummy.get_status_bit(bit=5) - assert after != before assert dummy.get_status_bit(bit=13) == 1 # position reached assert dummy.get_status_bit(bit=4) == 1 # command successfully processed + + +def test_dummy_performs_break_test(): + dummy = Dummy() + control_double_word = "00000040" # Bit 30 + set_position = "00000000" + set_speed = "00000000" + gripping_force = "00000000" + command = { + "inst": dummy.plc_output, + "value": control_double_word + set_position + set_speed + gripping_force, + } + dummy.post(command) + assert dummy.get_status_bit(bit=4) == 1 # command successfully processed diff --git a/schunk_egu_egk_gripper_tests/test/test_services.py b/schunk_egu_egk_gripper_tests/test/test_services.py index b169c5a..71bdd93 100644 --- a/schunk_egu_egk_gripper_tests/test/test_services.py +++ b/schunk_egu_egk_gripper_tests/test/test_services.py @@ -3,6 +3,7 @@ from test.helpers import check_each_in from schunk_egu_egk_gripper_interfaces.srv import ( # type: ignore[attr-defined] Acknowledge, + BrakeTest, ) from test.helpers import ServiceReturnsResult @@ -28,3 +29,10 @@ def test_driver_is_ready_after_acknowledge(running_driver): service = ServiceReturnsResult("/acknowledge", Acknowledge, Acknowledge.Request()) service.event.wait(timeout=1) assert service.result.success is True + + +@pytest.mark.launch(fixture=launch_description) +def test_driver_supports_break_test(running_driver): + service = ServiceReturnsResult("/brake_test", BrakeTest, BrakeTest.Request()) + service.event.wait(timeout=1) + assert service.result.brake_test_successful is True From d3184025e33bd92fc7d2e665768863043d0cb334 Mon Sep 17 00:00:00 2001 From: Stefan Scherzinger Date: Fri, 9 Aug 2024 06:29:54 +0200 Subject: [PATCH 07/25] Add a test for the gripper's _fast stop_ service Also add a function to set control bits in the dummy. This avoids a lot of boilerplate code in tests. --- schunk_egu_egk_gripper_dummy/src/dummy.py | 17 ++++++++ .../tests/test_dummy.py | 40 ++++++------------- .../tests/test_plc_communication.py | 11 ++++- .../test/test_services.py | 8 ++++ 4 files changed, 48 insertions(+), 28 deletions(-) diff --git a/schunk_egu_egk_gripper_dummy/src/dummy.py b/schunk_egu_egk_gripper_dummy/src/dummy.py index 7e4e978..398bb04 100644 --- a/schunk_egu_egk_gripper_dummy/src/dummy.py +++ b/schunk_egu_egk_gripper_dummy/src/dummy.py @@ -223,6 +223,18 @@ def get_control_bit(self, bit: int) -> int | bool: byte_index, bit_index = divmod(bit, 8) return 1 if self.plc_output_buffer[byte_index] & (1 << bit_index) != 0 else 0 + def set_control_bit(self, bit: int, value: bool) -> bool: + if bit < 0 or bit > 31: + return False + if bit in self.reserved_control_bits: + return False + byte_index, bit_index = divmod(bit, 8) + if value: + self.plc_output_buffer[byte_index] |= 1 << bit_index + else: + self.plc_output_buffer[byte_index] &= ~(1 << bit_index) + return True + def get_target_position(self) -> float: return struct.unpack("f", self.plc_output_buffer[4:8])[0] @@ -260,6 +272,11 @@ def process_control_bits(self) -> None: if self.get_control_bit(30) == 1: self.set_status_bit(bit=4, value=True) + # Fast stop + if self.get_control_bit(0) == 0: # fail-safe behavior + self.set_status_bit(bit=7, value=True) + self.set_status_diagnostics("D9") + # Move to absolute position if self.get_control_bit(13) == 1: self.move( diff --git a/schunk_egu_egk_gripper_dummy/tests/test_dummy.py b/schunk_egu_egk_gripper_dummy/tests/test_dummy.py index 58f62c7..bf2f3f2 100644 --- a/schunk_egu_egk_gripper_dummy/tests/test_dummy.py +++ b/schunk_egu_egk_gripper_dummy/tests/test_dummy.py @@ -44,15 +44,8 @@ def test_dummy_starts_in_error_state(): def test_dummy_is_ready_after_acknowledge(): dummy = Dummy() - control_double_word = "04000000" # bit 2 - set_position = "00000000" - set_speed = "00000000" - gripping_force = "00000000" - command = { - "inst": dummy.plc_output, - "value": control_double_word + set_position + set_speed + gripping_force, - } - dummy.post(command) + dummy.set_control_bit(bit=2, value=True) + dummy.process_control_bits() assert dummy.get_status_bit(0) == 1 # ready assert dummy.get_status_bit(7) == 0 # no error assert dummy.get_status_error() == "0" @@ -62,15 +55,7 @@ def test_dummy_is_ready_after_acknowledge(): def test_dummy_always_toggles_command_received_bit(): dummy = Dummy() before = dummy.get_status_bit(bit=5) # command received toggle - control_double_word = "00000000" - set_position = "00000000" - set_speed = "00000000" - gripping_force = "00000000" - command = { - "inst": dummy.plc_output, - "value": control_double_word + set_position + set_speed + gripping_force, - } - dummy.post(command) + dummy.process_control_bits() after = dummy.get_status_bit(bit=5) assert after != before @@ -99,13 +84,14 @@ def test_dummy_moves_to_absolute_position(): def test_dummy_performs_break_test(): dummy = Dummy() - control_double_word = "00000040" # Bit 30 - set_position = "00000000" - set_speed = "00000000" - gripping_force = "00000000" - command = { - "inst": dummy.plc_output, - "value": control_double_word + set_position + set_speed + gripping_force, - } - dummy.post(command) + dummy.set_control_bit(bit=30, value=True) + dummy.process_control_bits() assert dummy.get_status_bit(bit=4) == 1 # command successfully processed + + +def test_dummy_performs_fast_stop(): + dummy = Dummy() + dummy.set_control_bit(bit=0, value=False) + dummy.process_control_bits() + assert dummy.get_status_bit(bit=7) == 1 # error + assert dummy.get_status_diagnostics() == "D9" # ERR_FAST_STOP diff --git a/schunk_egu_egk_gripper_dummy/tests/test_plc_communication.py b/schunk_egu_egk_gripper_dummy/tests/test_plc_communication.py index 310692f..f147afe 100644 --- a/schunk_egu_egk_gripper_dummy/tests/test_plc_communication.py +++ b/schunk_egu_egk_gripper_dummy/tests/test_plc_communication.py @@ -81,11 +81,13 @@ def test_dummy_rejects_writing_invalid_status_diagnostics(): assert not dummy.set_status_diagnostics(code) -def test_dummy_supports_reading_bits_in_plc_control(): +def test_dummy_supports_reading_and_writing_bits_in_plc_control(): dummy = Dummy() for bit in dummy.valid_control_bits: + dummy.set_control_bit(bit=bit, value=True) result = dummy.get_control_bit(bit=bit) assert isinstance(result, int) # successful calls get the bit's value + assert result == 1 def test_dummy_rejects_reading_reserved_control_bits(): @@ -95,6 +97,13 @@ def test_dummy_rejects_reading_reserved_control_bits(): assert not dummy.get_control_bit(bit) +def test_dummy_rejects_writing_reserved_control_bits(): + dummy = Dummy() + invalid_bits = [-1, 999] + for bit in invalid_bits + dummy.reserved_control_bits: + assert not dummy.set_control_bit(bit, True) + + def test_dummy_supports_toggling_status_bits(): dummy = Dummy() for bit in dummy.valid_status_bits: diff --git a/schunk_egu_egk_gripper_tests/test/test_services.py b/schunk_egu_egk_gripper_tests/test/test_services.py index 71bdd93..6b27ccf 100644 --- a/schunk_egu_egk_gripper_tests/test/test_services.py +++ b/schunk_egu_egk_gripper_tests/test/test_services.py @@ -4,6 +4,7 @@ from schunk_egu_egk_gripper_interfaces.srv import ( # type: ignore[attr-defined] Acknowledge, BrakeTest, + FastStop, ) from test.helpers import ServiceReturnsResult @@ -36,3 +37,10 @@ def test_driver_supports_break_test(running_driver): service = ServiceReturnsResult("/brake_test", BrakeTest, BrakeTest.Request()) service.event.wait(timeout=1) assert service.result.brake_test_successful is True + + +@pytest.mark.launch(fixture=launch_description) +def test_driver_supports_fast_stop(running_driver): + service = ServiceReturnsResult("/fast_stop", FastStop, FastStop.Request()) + service.event.wait(timeout=1) + assert service.result.fast_stopped is True From ea45beeb68d63cb8b4db4fefb2fb91809b0ee029 Mon Sep 17 00:00:00 2001 From: Stefan Scherzinger Date: Fri, 9 Aug 2024 08:36:34 +0200 Subject: [PATCH 08/25] Rename the result field of the trigger-based services Reporting the `success` bool flag is like an in-official convention. There's no need for repeating the service's name. --- .../src/schunk_gripper_wrapper.cpp | 34 +++++++++---------- .../src/gripper_example.cpp | 2 +- .../srv/BrakeTest.srv | 2 +- .../srv/ChangeIp.srv | 2 +- .../srv/FastStop.srv | 2 +- .../srv/PrepareForShutdown.srv | 2 +- .../srv/ReleaseForManualMovement.srv | 2 +- .../srv/Softreset.srv | 2 +- .../srv/Stop.srv | 2 +- .../test/test_services.py | 4 +-- 10 files changed, 27 insertions(+), 27 deletions(-) diff --git a/schunk_egu_egk_gripper_driver/src/schunk_gripper_wrapper.cpp b/schunk_egu_egk_gripper_driver/src/schunk_gripper_wrapper.cpp index 5758742..a819099 100644 --- a/schunk_egu_egk_gripper_driver/src/schunk_gripper_wrapper.cpp +++ b/schunk_egu_egk_gripper_driver/src/schunk_gripper_wrapper.cpp @@ -1195,7 +1195,7 @@ void SchunkGripperNode::brake_test_srv(const std::shared_ptr } if((gripperBitInput(SUCCESS) == true) && (handshake != gripperBitInput(COMMAND_RECEIVED_TOGGLE))) { - res->brake_test_successful = true; + res->success = true; res->error_code = error_str; RCLCPP_INFO(this->get_logger(),"Brake test successful!"); } @@ -1204,7 +1204,7 @@ void SchunkGripperNode::brake_test_srv(const std::shared_ptr runGets(); gripper_updater->force_update(); res->error_code = error_str; - res->brake_test_successful = false; + res->success = false; RCLCPP_INFO(this->get_logger(),"Brake test failed!"); } @@ -1213,7 +1213,7 @@ void SchunkGripperNode::brake_test_srv(const std::shared_ptr { connection_error = server_response; res->error_code = server_response; - res->brake_test_successful = false; + res->success = false; RCLCPP_ERROR(this->get_logger(), "Failed Connection! %s", connection_error.c_str()); } @@ -1224,16 +1224,16 @@ void SchunkGripperNode::change_ip_srv(const std::shared_ptr r try { std::lock_guard lock(lock_mutex); - res->ip_changed = changeIp(req->new_ip); + res->success = changeIp(req->new_ip); - if(res->ip_changed == false) + if(res->success == false) { connection_error = "No gripper found. Using old IP"; gripper_updater->force_update(); return; } - if(res->ip_changed == true) + if(res->success == true) { this->set_parameter(rclcpp::Parameter("IP", req->new_ip)); } @@ -1290,12 +1290,12 @@ void SchunkGripperNode::stop_srv(const std::shared_ptr, std::shar if((gripperBitInput(SUCCESS) == 1) && (handshake != gripperBitInput(COMMAND_RECEIVED_TOGGLE))) { - res->stopped = 1; + res->success = true; RCLCPP_WARN(this->get_logger(),"Stopped!"); } else { - res->stopped = 0; + res->success = false; last_command = 0; } @@ -1323,12 +1323,12 @@ void SchunkGripperNode::fast_stop_srv(const std::shared_ptr, if(gripperBitInput(GRIPPER_ERROR)) { - res->fast_stopped = 1; + res->success = true; RCLCPP_WARN(this->get_logger(),"Fast stopped!"); } else { - res->fast_stopped = 0; + res->success = false; RCLCPP_WARN(this->get_logger(),"Fast stop failed!"); } } @@ -1336,7 +1336,7 @@ void SchunkGripperNode::fast_stop_srv(const std::shared_ptr, { connection_error = server_err; RCLCPP_ERROR(this->get_logger(), "Failed Connection! %s", connection_error.c_str()); - res->fast_stopped = 0; + res->success = false; RCLCPP_WARN(this->get_logger(), "Fast stop failed!"); } gripper_updater->force_update(); @@ -1388,12 +1388,12 @@ void SchunkGripperNode::softreset_srv(const std::shared_ptr, if(connection_once_lost == true && rclcpp::ok() && connection_error == "OK") { RCLCPP_INFO(this->get_logger(), "Softreset succeeded!"); - res->reset_success = true; + res->success = true; } else { RCLCPP_INFO(this->get_logger(), "Softreset failed!"); - res->reset_success = false; + res->success = false; } lock.unlock(); gripper_updater->force_update(); @@ -1428,12 +1428,12 @@ void SchunkGripperNode::prepare_for_shutdown_srv(const std::shared_ptrget_logger(),"READY FOR SHUTDOWN"); - res->prepared_for_shutdown = true; + res->success = true; } else { RCLCPP_INFO(this->get_logger(),"COMMAND FAILED"); - res->prepared_for_shutdown = false; + res->success = false; } gripper_updater->force_update(); @@ -1493,12 +1493,12 @@ void SchunkGripperNode::releaseForManualMov_srv(const std::shared_ptrget_logger(),"YOU CAN TAKE THE WORKPIECE!\n"); RCLCPP_WARN(this->get_logger(),"If you want to end this mode, perform a fast stop and acknowledge!"); - res->released_for_manual_movement = true; + res->success = true; } else { RCLCPP_WARN(this->get_logger(),"COMMAND FAILED"); - res->released_for_manual_movement = false; + res->success = false; } gripper_updater->force_update(); } diff --git a/schunk_egu_egk_gripper_examples/src/gripper_example.cpp b/schunk_egu_egk_gripper_examples/src/gripper_example.cpp index 2aea3cc..ae5e386 100644 --- a/schunk_egu_egk_gripper_examples/src/gripper_example.cpp +++ b/schunk_egu_egk_gripper_examples/src/gripper_example.cpp @@ -234,7 +234,7 @@ void moveRelativeAndStop(rclcpp_action::Client::SharedPt auto stop_req = std::make_shared(); auto resp = stop_client->async_send_request(stop_req); - bool stopped = resp.get()->stopped; + bool stopped = resp.get()->success; RCLCPP_INFO(rclcpp::get_logger("schunk_gripper_example"), "%s", stopped ? "Stopped!" : "Not stopped!"); auto res = move_rel_client->async_get_result(goal_handle.get()); diff --git a/schunk_egu_egk_gripper_interfaces/srv/BrakeTest.srv b/schunk_egu_egk_gripper_interfaces/srv/BrakeTest.srv index e0cac6d..7e0b3bf 100644 --- a/schunk_egu_egk_gripper_interfaces/srv/BrakeTest.srv +++ b/schunk_egu_egk_gripper_interfaces/srv/BrakeTest.srv @@ -1,3 +1,3 @@ --- -bool brake_test_successful +bool success string error_code diff --git a/schunk_egu_egk_gripper_interfaces/srv/ChangeIp.srv b/schunk_egu_egk_gripper_interfaces/srv/ChangeIp.srv index 6308764..4052df6 100644 --- a/schunk_egu_egk_gripper_interfaces/srv/ChangeIp.srv +++ b/schunk_egu_egk_gripper_interfaces/srv/ChangeIp.srv @@ -1,3 +1,3 @@ string new_ip --- -bool ip_changed +bool success diff --git a/schunk_egu_egk_gripper_interfaces/srv/FastStop.srv b/schunk_egu_egk_gripper_interfaces/srv/FastStop.srv index 4d1c0b1..410e0f9 100644 --- a/schunk_egu_egk_gripper_interfaces/srv/FastStop.srv +++ b/schunk_egu_egk_gripper_interfaces/srv/FastStop.srv @@ -1,2 +1,2 @@ --- -bool fast_stopped +bool success diff --git a/schunk_egu_egk_gripper_interfaces/srv/PrepareForShutdown.srv b/schunk_egu_egk_gripper_interfaces/srv/PrepareForShutdown.srv index bbc46d3..410e0f9 100644 --- a/schunk_egu_egk_gripper_interfaces/srv/PrepareForShutdown.srv +++ b/schunk_egu_egk_gripper_interfaces/srv/PrepareForShutdown.srv @@ -1,2 +1,2 @@ --- -bool prepared_for_shutdown +bool success diff --git a/schunk_egu_egk_gripper_interfaces/srv/ReleaseForManualMovement.srv b/schunk_egu_egk_gripper_interfaces/srv/ReleaseForManualMovement.srv index f2fb068..410e0f9 100644 --- a/schunk_egu_egk_gripper_interfaces/srv/ReleaseForManualMovement.srv +++ b/schunk_egu_egk_gripper_interfaces/srv/ReleaseForManualMovement.srv @@ -1,2 +1,2 @@ --- -bool released_for_manual_movement +bool success diff --git a/schunk_egu_egk_gripper_interfaces/srv/Softreset.srv b/schunk_egu_egk_gripper_interfaces/srv/Softreset.srv index 4102fa4..410e0f9 100644 --- a/schunk_egu_egk_gripper_interfaces/srv/Softreset.srv +++ b/schunk_egu_egk_gripper_interfaces/srv/Softreset.srv @@ -1,2 +1,2 @@ --- -bool reset_success +bool success diff --git a/schunk_egu_egk_gripper_interfaces/srv/Stop.srv b/schunk_egu_egk_gripper_interfaces/srv/Stop.srv index 91a705f..410e0f9 100644 --- a/schunk_egu_egk_gripper_interfaces/srv/Stop.srv +++ b/schunk_egu_egk_gripper_interfaces/srv/Stop.srv @@ -1,2 +1,2 @@ --- -bool stopped +bool success diff --git a/schunk_egu_egk_gripper_tests/test/test_services.py b/schunk_egu_egk_gripper_tests/test/test_services.py index 6b27ccf..1f0f391 100644 --- a/schunk_egu_egk_gripper_tests/test/test_services.py +++ b/schunk_egu_egk_gripper_tests/test/test_services.py @@ -36,11 +36,11 @@ def test_driver_is_ready_after_acknowledge(running_driver): def test_driver_supports_break_test(running_driver): service = ServiceReturnsResult("/brake_test", BrakeTest, BrakeTest.Request()) service.event.wait(timeout=1) - assert service.result.brake_test_successful is True + assert service.result.success is True @pytest.mark.launch(fixture=launch_description) def test_driver_supports_fast_stop(running_driver): service = ServiceReturnsResult("/fast_stop", FastStop, FastStop.Request()) service.event.wait(timeout=1) - assert service.result.fast_stopped is True + assert service.result.success is True From ddaca59f95b89fcbc4faae5530f451801c581f26 Mon Sep 17 00:00:00 2001 From: Stefan Scherzinger Date: Fri, 9 Aug 2024 09:35:04 +0200 Subject: [PATCH 09/25] Add a test for the gripper's _stop_ service --- schunk_egu_egk_gripper_dummy/src/dummy.py | 5 +++++ schunk_egu_egk_gripper_dummy/tests/test_dummy.py | 10 +++++++++- schunk_egu_egk_gripper_tests/test/test_services.py | 8 ++++++++ 3 files changed, 22 insertions(+), 1 deletion(-) diff --git a/schunk_egu_egk_gripper_dummy/src/dummy.py b/schunk_egu_egk_gripper_dummy/src/dummy.py index 398bb04..847e2df 100644 --- a/schunk_egu_egk_gripper_dummy/src/dummy.py +++ b/schunk_egu_egk_gripper_dummy/src/dummy.py @@ -277,6 +277,11 @@ def process_control_bits(self) -> None: self.set_status_bit(bit=7, value=True) self.set_status_diagnostics("D9") + # Controlled stop + if self.get_control_bit(1) == 1: + self.set_status_bit(bit=13, value=True) + self.set_status_bit(bit=4, value=True) + # Move to absolute position if self.get_control_bit(13) == 1: self.move( diff --git a/schunk_egu_egk_gripper_dummy/tests/test_dummy.py b/schunk_egu_egk_gripper_dummy/tests/test_dummy.py index bf2f3f2..a5c0584 100644 --- a/schunk_egu_egk_gripper_dummy/tests/test_dummy.py +++ b/schunk_egu_egk_gripper_dummy/tests/test_dummy.py @@ -91,7 +91,15 @@ def test_dummy_performs_break_test(): def test_dummy_performs_fast_stop(): dummy = Dummy() - dummy.set_control_bit(bit=0, value=False) + dummy.set_control_bit(bit=0, value=False) # fail-safe behavior dummy.process_control_bits() assert dummy.get_status_bit(bit=7) == 1 # error assert dummy.get_status_diagnostics() == "D9" # ERR_FAST_STOP + + +def test_dummy_performs_controlled_stop(): + dummy = Dummy() + dummy.set_control_bit(bit=1, value=True) + dummy.process_control_bits() + assert dummy.get_status_bit(bit=13) == 1 # position reached + assert dummy.get_status_bit(bit=4) == 1 # command successfully processed diff --git a/schunk_egu_egk_gripper_tests/test/test_services.py b/schunk_egu_egk_gripper_tests/test/test_services.py index 1f0f391..b056750 100644 --- a/schunk_egu_egk_gripper_tests/test/test_services.py +++ b/schunk_egu_egk_gripper_tests/test/test_services.py @@ -5,6 +5,7 @@ Acknowledge, BrakeTest, FastStop, + Stop, ) from test.helpers import ServiceReturnsResult @@ -44,3 +45,10 @@ def test_driver_supports_fast_stop(running_driver): service = ServiceReturnsResult("/fast_stop", FastStop, FastStop.Request()) service.event.wait(timeout=1) assert service.result.success is True + + +@pytest.mark.launch(fixture=launch_description) +def test_driver_supports_stop(running_driver): + service = ServiceReturnsResult("/stop", Stop, Stop.Request()) + service.event.wait(timeout=1) + assert service.result.success is True From 948122fe1e5a45c385ec39cf4e5ffbe96ea90d19 Mon Sep 17 00:00:00 2001 From: Stefan Scherzinger Date: Fri, 9 Aug 2024 10:43:58 +0200 Subject: [PATCH 10/25] Add a test for the gripper's _manual release_ service --- schunk_egu_egk_gripper_dummy/src/dummy.py | 11 +++++++++++ .../tests/test_dummy.py | 18 ++++++++++++++++++ .../test/test_services.py | 12 ++++++++++++ 3 files changed, 41 insertions(+) diff --git a/schunk_egu_egk_gripper_dummy/src/dummy.py b/schunk_egu_egk_gripper_dummy/src/dummy.py index 847e2df..ff70357 100644 --- a/schunk_egu_egk_gripper_dummy/src/dummy.py +++ b/schunk_egu_egk_gripper_dummy/src/dummy.py @@ -258,6 +258,12 @@ def get_actual_speed(self) -> float: return struct.unpack("f", bytes.fromhex(read_speed))[0] def process_control_bits(self) -> None: + """ + See the gripper's firmware documentation for EtherNet/IP [1]: + https://stb.cloud.schunk.com/media/IM0046706.PDF + + """ + # Command received toggle self.toggle_status_bit(bit=5) @@ -282,6 +288,11 @@ def process_control_bits(self) -> None: self.set_status_bit(bit=13, value=True) self.set_status_bit(bit=4, value=True) + # Manual release + if self.get_control_bit(5) == 1: + if self.get_status_bit(7) == 1: + self.set_status_bit(bit=8, value=True) + # Move to absolute position if self.get_control_bit(13) == 1: self.move( diff --git a/schunk_egu_egk_gripper_dummy/tests/test_dummy.py b/schunk_egu_egk_gripper_dummy/tests/test_dummy.py index a5c0584..b372ad3 100644 --- a/schunk_egu_egk_gripper_dummy/tests/test_dummy.py +++ b/schunk_egu_egk_gripper_dummy/tests/test_dummy.py @@ -103,3 +103,21 @@ def test_dummy_performs_controlled_stop(): dummy.process_control_bits() assert dummy.get_status_bit(bit=13) == 1 # position reached assert dummy.get_status_bit(bit=4) == 1 # command successfully processed + + +def test_dummy_supports_manual_release(): + dummy = Dummy() + + # Reject when not in error state + dummy.set_status_bit(bit=7, value=False) # clear error + dummy.set_status_diagnostics("00") + dummy.set_control_bit(bit=5, value=True) # release + dummy.process_control_bits() + assert dummy.get_status_bit(bit=8) == 0 # not released + + # Accept when in error state + dummy.set_control_bit(bit=0, value=False) # trigger fast stop + dummy.process_control_bits() + dummy.set_control_bit(bit=5, value=True) # release + dummy.process_control_bits() + assert dummy.get_status_bit(bit=8) == 1 # released for manual movement diff --git a/schunk_egu_egk_gripper_tests/test/test_services.py b/schunk_egu_egk_gripper_tests/test/test_services.py index b056750..d0d4bd2 100644 --- a/schunk_egu_egk_gripper_tests/test/test_services.py +++ b/schunk_egu_egk_gripper_tests/test/test_services.py @@ -6,6 +6,7 @@ BrakeTest, FastStop, Stop, + ReleaseForManualMovement, ) from test.helpers import ServiceReturnsResult @@ -47,6 +48,17 @@ def test_driver_supports_fast_stop(running_driver): assert service.result.success is True +@pytest.mark.launch(fixture=launch_description) +def test_driver_supports_release_for_manual_movement(running_driver): + service = ServiceReturnsResult( + "/release_for_manual_movement", + ReleaseForManualMovement, + ReleaseForManualMovement.Request(), + ) + service.event.wait(timeout=1) + assert service.result.success is True + + @pytest.mark.launch(fixture=launch_description) def test_driver_supports_stop(running_driver): service = ServiceReturnsResult("/stop", Stop, Stop.Request()) From 3f0bd28de10337b477d5668a7826b303f37436d9 Mon Sep 17 00:00:00 2001 From: Stefan Scherzinger Date: Fri, 9 Aug 2024 14:11:27 +0200 Subject: [PATCH 11/25] Drop the changeIP (`reconnect`) service In ROS2, the gripper has a single IP address at startup. This service seems irrelevant for these driver's use cases and adds complexity that's difficult to test and maintain. We get it back once we really need this. The driver _does_ reconnect automatically when the connection drops, but it does it without assuming another IP. --- .../schunk_gripper_wrapper.hpp | 4 -- .../src/schunk_gripper_wrapper.cpp | 42 ------------------ .../src/gripper_example.cpp | 2 - .../CMakeLists.txt | 1 - .../srv/ChangeIp.srv | 3 -- .../schunk_gripper_lib.hpp | 1 - .../src/schunk_gripper_lib.cpp | 43 ------------------- .../test/test_services.py | 1 - 8 files changed, 97 deletions(-) delete mode 100644 schunk_egu_egk_gripper_interfaces/srv/ChangeIp.srv diff --git a/schunk_egu_egk_gripper_driver/include/schunk_egu_egk_gripper_driver/schunk_gripper_wrapper.hpp b/schunk_egu_egk_gripper_driver/include/schunk_egu_egk_gripper_driver/schunk_gripper_wrapper.hpp index f6924fa..d3356e1 100644 --- a/schunk_egu_egk_gripper_driver/include/schunk_egu_egk_gripper_driver/schunk_gripper_wrapper.hpp +++ b/schunk_egu_egk_gripper_driver/include/schunk_egu_egk_gripper_driver/schunk_gripper_wrapper.hpp @@ -47,7 +47,6 @@ #include "schunk_egu_egk_gripper_interfaces/action/release_workpiece.hpp" #include "schunk_egu_egk_gripper_interfaces/srv/release_for_manual_movement.hpp" #include "schunk_egu_egk_gripper_interfaces/srv/gripper_info.hpp" -#include "schunk_egu_egk_gripper_interfaces/srv/change_ip.hpp" #include "schunk_egu_egk_gripper_interfaces/action/grip.hpp" #include "schunk_egu_egk_gripper_interfaces/srv/parameter_get.hpp" #include "schunk_egu_egk_gripper_interfaces/srv/parameter_set.hpp" @@ -70,7 +69,6 @@ class SchunkGripperNode : public rclcpp::Node, public Gripper using Softreset = schunk_egu_egk_gripper_interfaces::srv::Softreset; using PrepareForShutdown = schunk_egu_egk_gripper_interfaces::srv::PrepareForShutdown; using GripperInfo= schunk_egu_egk_gripper_interfaces::srv::GripperInfo; - using ChangeIp = schunk_egu_egk_gripper_interfaces::srv::ChangeIp; using ParameterGet = schunk_egu_egk_gripper_interfaces::srv::ParameterGet; using ParameterSet = schunk_egu_egk_gripper_interfaces::srv::ParameterSet; @@ -145,7 +143,6 @@ class SchunkGripperNode : public rclcpp::Node, public Gripper void fast_stop_srv(const std::shared_ptr, std::shared_ptr); void parameter_get_srv(const std::shared_ptr, std::shared_ptr); void parameter_set_srv(const std::shared_ptr, std::shared_ptr); - void change_ip_srv(const std::shared_ptr, std::shared_ptr); void releaseForManualMov_srv(const std::shared_ptr, std::shared_ptr); void softreset_srv(const std::shared_ptr, std::shared_ptr); void prepare_for_shutdown_srv(const std::shared_ptr, std::shared_ptr); @@ -224,7 +221,6 @@ class SchunkGripperNode : public rclcpp::Node, public Gripper rclcpp::Service::SharedPtr softreset_service; rclcpp::Service::SharedPtr prepare_for_shutdown_service; rclcpp::Service::SharedPtr info_service; - rclcpp::Service::SharedPtr change_ip_service; }; #endif diff --git a/schunk_egu_egk_gripper_driver/src/schunk_gripper_wrapper.cpp b/schunk_egu_egk_gripper_driver/src/schunk_gripper_wrapper.cpp index a819099..8064d59 100644 --- a/schunk_egu_egk_gripper_driver/src/schunk_gripper_wrapper.cpp +++ b/schunk_egu_egk_gripper_driver/src/schunk_gripper_wrapper.cpp @@ -163,7 +163,6 @@ void SchunkGripperNode::advertiseServices() parameter_get_service = this->create_service("parameter_get", std::bind(&SchunkGripperNode::parameter_get_srv,this,std::placeholders::_1,std::placeholders::_2), rmw_qos_profile_services_default, services_group); parameter_set_service= this->create_service("parameter_set", std::bind(&SchunkGripperNode::parameter_set_srv,this,std::placeholders::_1,std::placeholders::_2), rmw_qos_profile_services_default, services_group); info_service = this->create_service("gripper_info", std::bind(&SchunkGripperNode::info_srv,this,std::placeholders::_1,std::placeholders::_2), rmw_qos_profile_services_default, rest); - change_ip_service = this->create_service("reconnect", std::bind(&SchunkGripperNode::change_ip_srv,this,std::placeholders::_1,std::placeholders::_2), rmw_qos_profile_services_default, services_group); } //Advertise Actions @@ -1218,48 +1217,7 @@ void SchunkGripperNode::brake_test_srv(const std::shared_ptr } } -//Ip change service, if the ip should change during runtime of the program -void SchunkGripperNode::change_ip_srv(const std::shared_ptr req, std::shared_ptr res) -{ - try - { - std::lock_guard lock(lock_mutex); - res->success = changeIp(req->new_ip); - if(res->success == false) - { - connection_error = "No gripper found. Using old IP"; - gripper_updater->force_update(); - return; - } - - if(res->success == true) - { - this->set_parameter(rclcpp::Parameter("IP", req->new_ip)); - } - - if(start_connection == false && ip_changed_with_all_param == true) - { - advertiseConnectionRelevant(); - start_connection = true; - } - - if(ip_changed_with_all_param == true) - { - this->set_parameter(rclcpp::Parameter("model", model)); - - if(connection_error == "OK") - { - reconnect(); - } - } - } - catch(...) - { - RCLCPP_ERROR(this->get_logger(), "Handshake failed!"); - } - -} //Stop service callback void SchunkGripperNode::stop_srv(const std::shared_ptr, std::shared_ptr res) { diff --git a/schunk_egu_egk_gripper_examples/src/gripper_example.cpp b/schunk_egu_egk_gripper_examples/src/gripper_example.cpp index ae5e386..d4e7c0c 100644 --- a/schunk_egu_egk_gripper_examples/src/gripper_example.cpp +++ b/schunk_egu_egk_gripper_examples/src/gripper_example.cpp @@ -36,7 +36,6 @@ #include "schunk_egu_egk_gripper_interfaces/srv/prepare_for_shutdown.hpp" #include "schunk_egu_egk_gripper_interfaces/srv/softreset.hpp" #include "schunk_egu_egk_gripper_interfaces/srv/brake_test.hpp" -#include "schunk_egu_egk_gripper_interfaces/srv/change_ip.hpp" #include "schunk_egu_egk_gripper_interfaces/srv/parameter_get.hpp" #include "schunk_egu_egk_gripper_interfaces/srv/parameter_set.hpp" #include "schunk_egu_egk_gripper_interfaces/srv/release_for_manual_movement.hpp" @@ -66,7 +65,6 @@ std::string name_space; using Softreset = schunk_egu_egk_gripper_interfaces::srv::Softreset; using PrepareForShutdown = schunk_egu_egk_gripper_interfaces::srv::PrepareForShutdown; using GripperInfo= schunk_egu_egk_gripper_interfaces::srv::GripperInfo; - using ChangeIp = schunk_egu_egk_gripper_interfaces::srv::ChangeIp; using ParameterGet = schunk_egu_egk_gripper_interfaces::srv::ParameterGet; using ParameterSet = schunk_egu_egk_gripper_interfaces::srv::ParameterSet; diff --git a/schunk_egu_egk_gripper_interfaces/CMakeLists.txt b/schunk_egu_egk_gripper_interfaces/CMakeLists.txt index 26e6ca1..6c87a1e 100644 --- a/schunk_egu_egk_gripper_interfaces/CMakeLists.txt +++ b/schunk_egu_egk_gripper_interfaces/CMakeLists.txt @@ -26,7 +26,6 @@ rosidl_generate_interfaces( msg/State.msg srv/Acknowledge.srv srv/BrakeTest.srv - srv/ChangeIp.srv srv/FastStop.srv srv/GripperInfo.srv srv/ParameterGet.srv diff --git a/schunk_egu_egk_gripper_interfaces/srv/ChangeIp.srv b/schunk_egu_egk_gripper_interfaces/srv/ChangeIp.srv deleted file mode 100644 index 4052df6..0000000 --- a/schunk_egu_egk_gripper_interfaces/srv/ChangeIp.srv +++ /dev/null @@ -1,3 +0,0 @@ -string new_ip ---- -bool success diff --git a/schunk_egu_egk_gripper_library/include/schunk_egu_egk_gripper_library/schunk_gripper_lib.hpp b/schunk_egu_egk_gripper_library/include/schunk_egu_egk_gripper_library/schunk_gripper_lib.hpp index a52b69c..0c88a8a 100644 --- a/schunk_egu_egk_gripper_library/include/schunk_egu_egk_gripper_library/schunk_gripper_lib.hpp +++ b/schunk_egu_egk_gripper_library/include/schunk_egu_egk_gripper_library/schunk_gripper_lib.hpp @@ -56,7 +56,6 @@ class Gripper : protected AnybusCom bool gripperBitOutput(const uint32_t&) const; //retrieve individual bits from the control double word void acknowledge(); //acknowledge - bool changeIp(const std::string &); //changeIP bool ip_changed_with_all_param; //IP and parameter changed diff --git a/schunk_egu_egk_gripper_library/src/schunk_gripper_lib.cpp b/schunk_egu_egk_gripper_library/src/schunk_gripper_lib.cpp index d3c4234..537b309 100644 --- a/schunk_egu_egk_gripper_library/src/schunk_gripper_lib.cpp +++ b/schunk_egu_egk_gripper_library/src/schunk_gripper_lib.cpp @@ -319,50 +319,7 @@ void Gripper::acknowledge() getWithInstance(PLC_SYNC_INPUT_INST); plc_sync_output[0] &= mask; } -//change the ip-address -bool Gripper::changeIp(const std::string &new_ip) -{ - if(new_ip.size() > 100) return false; - - std::string old_ip = ip; - ip = new_ip; - std::string old_model = model; - initAddresses(); - //Control if it is the same gripper - try - { - startGripper(); - - ip_changed_with_all_param = true; - - return true; - } - catch(const char* res) - { - ip_changed_with_all_param = false; - std::cout << "No Gripper found. New IP: " << ip << std::endl; - initAddresses(); - return true; - } - catch(const nlohmann::json::parse_error &e) - { - ip_changed_with_all_param = true; - ip = old_ip; - std::cout << "message: " << e.what() << "\nexception id: " << e.id << std::endl; - std::cout << "Setting to old IP: " << ip << std::endl; - initAddresses(); - return false; - } - catch(const std::exception &e) - { - ip_changed_with_all_param = true; - ip = old_ip; - std::cout << "Wrong data found. Setting to old IP: " << ip << std::endl; - initAddresses(); - return false; - } -} //send action directly after current (cyclic-) http or immediately void Gripper::sendAction() { diff --git a/schunk_egu_egk_gripper_tests/test/test_services.py b/schunk_egu_egk_gripper_tests/test/test_services.py index d0d4bd2..1ebb535 100644 --- a/schunk_egu_egk_gripper_tests/test/test_services.py +++ b/schunk_egu_egk_gripper_tests/test/test_services.py @@ -19,7 +19,6 @@ def test_driver_advertices_all_relevant_services(running_driver): "/fast_stop", "/gripper_info", "/prepare_for_shutdown", - "/reconnect", "/release_for_manual_movement", "/softreset", "/stop", From c8b5a72e6816fb7c8950f688993e5b0fda5be228 Mon Sep 17 00:00:00 2001 From: Stefan Scherzinger Date: Fri, 9 Aug 2024 14:56:35 +0200 Subject: [PATCH 12/25] Fix the `gripper_info` service The terminal log output was not easily accessible from client code. We now return the collected information in a result string. --- .../schunk_gripper_wrapper.hpp | 3 +- .../src/schunk_gripper_wrapper.cpp | 77 ++++++++++--------- .../srv/GripperInfo.srv | 2 + .../test/test_services.py | 9 +++ 4 files changed, 52 insertions(+), 39 deletions(-) diff --git a/schunk_egu_egk_gripper_driver/include/schunk_egu_egk_gripper_driver/schunk_gripper_wrapper.hpp b/schunk_egu_egk_gripper_driver/include/schunk_egu_egk_gripper_driver/schunk_gripper_wrapper.hpp index d3356e1..dd9173f 100644 --- a/schunk_egu_egk_gripper_driver/include/schunk_egu_egk_gripper_driver/schunk_gripper_wrapper.hpp +++ b/schunk_egu_egk_gripper_driver/include/schunk_egu_egk_gripper_driver/schunk_gripper_wrapper.hpp @@ -146,7 +146,7 @@ class SchunkGripperNode : public rclcpp::Node, public Gripper void releaseForManualMov_srv(const std::shared_ptr, std::shared_ptr); void softreset_srv(const std::shared_ptr, std::shared_ptr); void prepare_for_shutdown_srv(const std::shared_ptr, std::shared_ptr); - void info_srv(const std::shared_ptr, std::shared_ptr); + void info_srv(const std::shared_ptr, std::shared_ptr res); //Action-basic-functions template @@ -195,7 +195,6 @@ class SchunkGripperNode : public rclcpp::Node, public Gripper rclcpp::CallbackGroup::SharedPtr messages_group; rclcpp::CallbackGroup::SharedPtr services_group; rclcpp::CallbackGroup::SharedPtr actions_group; - rclcpp::CallbackGroup::SharedPtr rest; public: diff --git a/schunk_egu_egk_gripper_driver/src/schunk_gripper_wrapper.cpp b/schunk_egu_egk_gripper_driver/src/schunk_gripper_wrapper.cpp index 8064d59..0801f57 100644 --- a/schunk_egu_egk_gripper_driver/src/schunk_gripper_wrapper.cpp +++ b/schunk_egu_egk_gripper_driver/src/schunk_gripper_wrapper.cpp @@ -55,13 +55,10 @@ SchunkGripperNode::SchunkGripperNode(const rclcpp::NodeOptions &options) : services_group = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); // actions_group = this->create_callback_group(rclcpp::CallbackGroupType::Reentrant); - // rest = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); rclcpp::PublisherOptions option_messages; option_messages.callback_group = messages_group; - rclcpp::SubscriptionOptions option; - option.callback_group = rest; //ParameterDescription read_only rcl_interfaces::msg::ParameterDescriptor paramDesc; paramDesc.read_only = true; @@ -162,7 +159,7 @@ void SchunkGripperNode::advertiseServices() fast_stop_service = this->create_service("fast_stop", std::bind(&SchunkGripperNode::fast_stop_srv,this,std::placeholders::_1,std::placeholders::_2), rmw_qos_profile_services_default, services_group); parameter_get_service = this->create_service("parameter_get", std::bind(&SchunkGripperNode::parameter_get_srv,this,std::placeholders::_1,std::placeholders::_2), rmw_qos_profile_services_default, services_group); parameter_set_service= this->create_service("parameter_set", std::bind(&SchunkGripperNode::parameter_set_srv,this,std::placeholders::_1,std::placeholders::_2), rmw_qos_profile_services_default, services_group); - info_service = this->create_service("gripper_info", std::bind(&SchunkGripperNode::info_srv,this,std::placeholders::_1,std::placeholders::_2), rmw_qos_profile_services_default, rest); + info_service = this->create_service("gripper_info", std::bind(&SchunkGripperNode::info_srv,this,std::placeholders::_1,std::placeholders::_2), rmw_qos_profile_services_default, services_group); } //Advertise Actions @@ -1461,20 +1458,23 @@ void SchunkGripperNode::releaseForManualMov_srv(const std::shared_ptrforce_update(); } //Get infos of the gripper service -void SchunkGripperNode::info_srv(const std::shared_ptr, std::shared_ptr) +void SchunkGripperNode::info_srv(const std::shared_ptr, std::shared_ptr res) { + auto info_stream = std::stringstream(); const std::lock_guard lock(lock_mutex); try { - if(wrong_version) RCLCPP_WARN_STREAM(this->get_logger(),"Using not suitable software-version. Some informations may be misleading"); - + if(wrong_version) + { + info_stream << "Using not suitable software-version. Some informations may be misleading"; + } std::vector char_strings; uint32_t data; uint16_t data2; float data3; - RCLCPP_INFO_STREAM(this->get_logger(),"\n\n\nGRIPPER TYPE: " << model.c_str() - << "\nIP: " << ip << std::endl); + info_stream << "\n\n\nGRIPPER TYPE: " << model.c_str() + << "\nIP: " << ip << std::endl; getEnums(FIELDBUS_TYPE_INST,fieldbus_type); getParameter(MAC_ADDR_INST, 6, CHAR_DATA); @@ -1482,10 +1482,9 @@ void SchunkGripperNode::info_srv(const std::shared_ptr, st for(size_t i = 0; i < 6; i++){mac[i] = static_cast(static_cast(char_vector.at(i)));} - RCLCPP_INFO_STREAM(this->get_logger(), - "\nFieldbustype: " << json_data["string"] + info_stream << "\nFieldbustype: " << json_data["string"] << "\nMac-address: " << std::hex << mac[0] << ":" << mac[1] << ":" << mac[2] << ":" << mac[3] << ":" - << mac[4] << ":" << mac[5] << std::endl); + << mac[4] << ":" << mac[5] << std::endl; //Get the char-Parameter and save them as strings in char_strings @@ -1503,64 +1502,64 @@ void SchunkGripperNode::info_srv(const std::shared_ptr, st getWithInstance(SERIAL_NO_NUM_INST, &data); getWithInstance(SW_VERSION_NUM_INST, &data2); - RCLCPP_INFO_STREAM(this->get_logger(),"\nSerial number text: " << char_strings[0] + info_stream << "\nSerial number text: " << char_strings[0] <<"\nOrder number text: " << char_strings[1] <<"\nDevice serial number encoded: " << data <<"\nMain software build date: " << char_strings[2] <<"\nMain software build time: " << char_strings[3] <<"\nMain software version short: " << data2 <<"\nMain software version: " << char_strings[4] - <<"\nCommunication software version: " << comm_version << std::endl); + <<"\nCommunication software version: " << comm_version << std::endl; getWithInstance(UPTIME_INST, &data); - RCLCPP_INFO_STREAM(this->get_logger(),"\nSystem uptime: " << data << " s" << std::endl); + info_stream << "\nSystem uptime: " << data << " s" << std::endl; getWithInstance(DEAD_LOAD_KG_INST, &data3); - RCLCPP_INFO_STREAM(this->get_logger(),"\nNet mass of the Gripper: " << data3 << " kg" << std::endl); + info_stream << "\nNet mass of the Gripper: " << data3 << " kg" << std::endl; getParameter(TOOL_CENT_POINT_INST, 6, FLOAT_DATA); - RCLCPP_INFO_STREAM(this->get_logger(),"\nTool center point 6D-Frame: \n" + info_stream << "\nTool center point 6D-Frame: \n" << float_vector[0] << " mm " << float_vector[1] << " mm " << float_vector[2] << " mm\n" - << float_vector[3] << " " << float_vector[4] << " " << float_vector[5] << std::endl); + << float_vector[3] << " " << float_vector[4] << " " << float_vector[5] << std::endl; getParameter(CENT_OF_MASS_INST, 6, FLOAT_DATA); - RCLCPP_INFO_STREAM(this->get_logger(), "\nCenter of Mass 6D-frame: \n" + info_stream << "\nCenter of Mass 6D-frame: \n" << float_vector[0] << " mm " << float_vector[1] << " mm " << float_vector[2] << " mm\n" - << float_vector[3] << " kg*m^2 " << float_vector[4] << " kg*m^2 "<< float_vector[5] << " kg*m^2"<< std::endl); + << float_vector[3] << " kg*m^2 " << float_vector[4] << " kg*m^2 "<< float_vector[5] << " kg*m^2"<< std::endl; - RCLCPP_INFO_STREAM(this->get_logger(),"\nMin. absolute position: " << min_pos << " mm\n" + info_stream << "\nMin. absolute position: " << min_pos << " mm\n" << "Max. absolute position: " << max_pos << " mm\n" - << "Zero_pos_offset: " << zero_pos_ofs << " mm" << std::endl); + << "Zero_pos_offset: " << zero_pos_ofs << " mm" << std::endl; - RCLCPP_INFO_STREAM(this->get_logger(),"\nMin. velocity_of_movement: " << min_vel << " mm/s\n" - << "Max. velocity_of_movement: " << max_vel << " mm/s" <get_logger(),"\nMax. grip velocity_of_movement: "<< max_grp_vel << " mm/s\n" + info_stream << "\nMax. grip velocity_of_movement: "<< max_grp_vel << " mm/s\n" << "Min. grip force: " << min_grip_force << " N\n" - << "Max. grip force: " << max_grip_force << " N" << std::endl); + << "Max. grip force: " << max_grip_force << " N" << std::endl; if(model.find("EGU") != std::string::npos) { - RCLCPP_INFO_STREAM(this->get_logger(),"\nMax allowed grip force StrongGrip: " << max_allow_force << " N " << std::endl); + info_stream << "\nMax allowed grip force StrongGrip: " << max_allow_force << " N " << std::endl; } getWithInstance(USED_CUR_LIMIT_INST, &data3); - RCLCPP_INFO_STREAM(this->get_logger(),"\nUsed current limit: " << data3 << " A" << std::endl); + info_stream << "\nUsed current limit: " << data3 << " A" << std::endl; getWithInstance(MAX_PHYS_STROKE_INST, &data3); - RCLCPP_INFO_STREAM(this->get_logger(),"Max. physical stroke: " << data3 << " mm" << std::endl); + info_stream << "Max. physical stroke: " << data3 << " mm" << std::endl; getWithOffset(MIN_ERR_MOT_VOLT_OFFSET, 6, float_vector); - RCLCPP_INFO_STREAM(this->get_logger(),"\nMin. error motor voltage: " << float_vector[0] << " V\n" + info_stream << "\nMin. error motor voltage: " << float_vector[0] << " V\n" << "Max. error motor voltage: " << float_vector[1] << " V\n" << "Min. error logic voltage: " << float_vector[2] << " V\n" << "Max. error logic voltage: " << float_vector[3] << " V\n" << "Min. error logic temperature: " << float_vector[4] << " C\n" - << "Max. error logic temperature: " << float_vector[5] << " C" << std::endl); + << "Max. error logic temperature: " << float_vector[5] << " C" << std::endl; getWithInstance(MEAS_LGC_TEMP_INST, &data3); - RCLCPP_INFO_STREAM(this->get_logger(), "Measured logic temperature: " << data3 << " C" << std::endl); + info_stream << "Measured logic temperature: " << data3 << " C" << std::endl; getWithOffset(MEAS_LGC_VOLT_OFFSET, 8, float_vector); - RCLCPP_INFO_STREAM(this->get_logger(), + info_stream << "\nMeasured logic voltage: " << float_vector[0] << " V\n" << "Measured motor voltage: " << float_vector[1] << " V\n" << "Min. warning motor voltage: " << float_vector[2] << " V\n" @@ -1568,16 +1567,20 @@ void SchunkGripperNode::info_srv(const std::shared_ptr, st << "Min. warning logic voltage: " << float_vector[4] << " V\n" << "Max. warning logic voltage: " << float_vector[5] << " V\n" << "Min. warning logic temperature: " << float_vector[6] << " C\n" - << "Max. warning logic temperature: " << float_vector[7] << " C" << std::endl); + << "Max. warning logic temperature: " << float_vector[7] << " C" << std::endl; + res->info = info_stream.str(); + res->success = true; } - catch(const char* res) + catch(const char* error) { - connection_error = res; - RCLCPP_ERROR(this->get_logger(), "Failed Connection! %s", connection_error.c_str()); + connection_error = error; + res->success = false; + RCLCPP_ERROR(this->get_logger(), "Failed Connection! %s", connection_error.c_str()); } catch(const std::exception &e) { + res->success = false; RCLCPP_ERROR(this->get_logger(), "%s", e.what()); } } diff --git a/schunk_egu_egk_gripper_interfaces/srv/GripperInfo.srv b/schunk_egu_egk_gripper_interfaces/srv/GripperInfo.srv index ed97d53..fafb7ac 100644 --- a/schunk_egu_egk_gripper_interfaces/srv/GripperInfo.srv +++ b/schunk_egu_egk_gripper_interfaces/srv/GripperInfo.srv @@ -1 +1,3 @@ --- +bool success +string info diff --git a/schunk_egu_egk_gripper_tests/test/test_services.py b/schunk_egu_egk_gripper_tests/test/test_services.py index 1ebb535..9437419 100644 --- a/schunk_egu_egk_gripper_tests/test/test_services.py +++ b/schunk_egu_egk_gripper_tests/test/test_services.py @@ -7,6 +7,7 @@ FastStop, Stop, ReleaseForManualMovement, + GripperInfo, ) from test.helpers import ServiceReturnsResult @@ -47,6 +48,14 @@ def test_driver_supports_fast_stop(running_driver): assert service.result.success is True +@pytest.mark.launch(fixture=launch_description) +def test_driver_supports_gripper_info(running_driver): + service = ServiceReturnsResult("/gripper_info", GripperInfo, GripperInfo.Request()) + service.event.wait(timeout=1) + assert service.result.success is True + assert service.result.info != "" + + @pytest.mark.launch(fixture=launch_description) def test_driver_supports_release_for_manual_movement(running_driver): service = ServiceReturnsResult( From 08a2e58255f97bae95a9eb5173236b4b29f5da0e Mon Sep 17 00:00:00 2001 From: Stefan Scherzinger Date: Fri, 9 Aug 2024 15:01:04 +0200 Subject: [PATCH 13/25] Update the readme Correct sections that mentioned the `reconnect` and the `gripper_info` services. --- README.md | 8 +------- 1 file changed, 1 insertion(+), 7 deletions(-) diff --git a/README.md b/README.md index 50dad2f..930184a 100644 --- a/README.md +++ b/README.md @@ -82,7 +82,7 @@ a) Using a component manager – this process is also outlined in the launch fil b) Within your custom executable, coupled with a (multithreaded-)executor. -If you prefer starting the node in your main function, ensure that you include `Schunk::schunk_egu_egk_gripper_driver` in CMake's `target_link_libraries()`. Additionally, always specify the IP address, setting the parameter overrides "IP" to your designated IP. Alternatively, create the `SchunkGripperNode`-Component and utilize the `reconnect` service, specifying your IP. +If you prefer starting the node in your main function, ensure that you include `Schunk::schunk_egu_egk_gripper_driver` in CMake's `target_link_libraries()`. Additionally, always specify the IP address, setting the parameter overrides "IP" to your designated IP. ## Actions All functionalities of the gripper, including movement, are treated as actions. This implies that when gripping, moving, or releasing a workpiece, you need to send a goal and can receive a result or feedback. Releasing a workpiece is the only action where you send an empty goal: @@ -108,10 +108,8 @@ Services are functionalities that do not involve movement or occur so rapidly th - `softreset` - `parameter_get` - `parameter_set` -- `reconnect` - `release_for_manual_movement` - `prepare_for_shutdown` -- `gripper_info` **Important:** During a soft reset, no topics will be published. This will last for approximately 7 seconds. Afterward, all publications resume, and you can modify parameters. @@ -121,10 +119,6 @@ Services are functionalities that do not involve movement or occur so rapidly th All other services can be used whenever you like. (**Note:** Fast stop is an abort of movement, so it always provokes an error). -`gripper_info` publishes some information about the gripper on the terminal screen. - -`reconnect` is the only method for altering the IP address during runtime. If nothing is connected to the IP address or a gripper is connected, it undergoes a change. If something else is linked to this IP, errors will occur, and the old address will be retained in such cases. Exercise caution when using this service! - With `parameter_get` and `parameter_set` you can read and set all allowed Parameter of the gripper. For getting and setting you need always the parameter instance. After that ## Parameters From 689f750a36e4b9dcfbd74334581efad839b70c0a Mon Sep 17 00:00:00 2001 From: Stefan Scherzinger Date: Fri, 9 Aug 2024 15:28:49 +0200 Subject: [PATCH 14/25] Add a test for the gripper's _prepare for shutdown_ service --- schunk_egu_egk_gripper_dummy/src/dummy.py | 4 ++++ schunk_egu_egk_gripper_dummy/tests/test_dummy.py | 7 +++++++ schunk_egu_egk_gripper_tests/test/test_services.py | 10 ++++++++++ 3 files changed, 21 insertions(+) diff --git a/schunk_egu_egk_gripper_dummy/src/dummy.py b/schunk_egu_egk_gripper_dummy/src/dummy.py index ff70357..6fe12d1 100644 --- a/schunk_egu_egk_gripper_dummy/src/dummy.py +++ b/schunk_egu_egk_gripper_dummy/src/dummy.py @@ -293,6 +293,10 @@ def process_control_bits(self) -> None: if self.get_status_bit(7) == 1: self.set_status_bit(bit=8, value=True) + # Shutdown + if self.get_control_bit(3) == 1: + self.set_status_bit(bit=2, value=True) + # Move to absolute position if self.get_control_bit(13) == 1: self.move( diff --git a/schunk_egu_egk_gripper_dummy/tests/test_dummy.py b/schunk_egu_egk_gripper_dummy/tests/test_dummy.py index b372ad3..a06c1d0 100644 --- a/schunk_egu_egk_gripper_dummy/tests/test_dummy.py +++ b/schunk_egu_egk_gripper_dummy/tests/test_dummy.py @@ -121,3 +121,10 @@ def test_dummy_supports_manual_release(): dummy.set_control_bit(bit=5, value=True) # release dummy.process_control_bits() assert dummy.get_status_bit(bit=8) == 1 # released for manual movement + + +def test_dummy_supports_prepare_for_shutdown(): + dummy = Dummy() + dummy.set_control_bit(bit=3, value=True) + dummy.process_control_bits() + assert dummy.get_status_bit(bit=2) == 1 # ready for shutdown diff --git a/schunk_egu_egk_gripper_tests/test/test_services.py b/schunk_egu_egk_gripper_tests/test/test_services.py index 9437419..fbe3b8e 100644 --- a/schunk_egu_egk_gripper_tests/test/test_services.py +++ b/schunk_egu_egk_gripper_tests/test/test_services.py @@ -8,6 +8,7 @@ Stop, ReleaseForManualMovement, GripperInfo, + PrepareForShutdown, ) from test.helpers import ServiceReturnsResult @@ -56,6 +57,15 @@ def test_driver_supports_gripper_info(running_driver): assert service.result.info != "" +@pytest.mark.launch(fixture=launch_description) +def test_driver_supports_prepare_for_shutdown(running_driver): + service = ServiceReturnsResult( + "/prepare_for_shutdown", PrepareForShutdown, PrepareForShutdown.Request() + ) + service.event.wait(timeout=1) + assert service.result.success is True + + @pytest.mark.launch(fixture=launch_description) def test_driver_supports_release_for_manual_movement(running_driver): service = ServiceReturnsResult( From b5e1b1b65a1360e5f6112869aed564bb2d166b73 Mon Sep 17 00:00:00 2001 From: Stefan Scherzinger Date: Mon, 12 Aug 2024 14:35:49 +0200 Subject: [PATCH 15/25] Add a test for the gripper's _release workpiece_ action Also simplify further action tests with a client wrapper. --- schunk_egu_egk_gripper_dummy/src/dummy.py | 7 +++++ .../tests/test_dummy.py | 11 ++++++++ schunk_egu_egk_gripper_tests/test/helpers.py | 26 +++++++++++++++++ .../test/test_actions.py | 28 +++++++++---------- 4 files changed, 57 insertions(+), 15 deletions(-) diff --git a/schunk_egu_egk_gripper_dummy/src/dummy.py b/schunk_egu_egk_gripper_dummy/src/dummy.py index 6fe12d1..88615ae 100644 --- a/schunk_egu_egk_gripper_dummy/src/dummy.py +++ b/schunk_egu_egk_gripper_dummy/src/dummy.py @@ -297,6 +297,13 @@ def process_control_bits(self) -> None: if self.get_control_bit(3) == 1: self.set_status_bit(bit=2, value=True) + # Release workpiece + if self.get_control_bit(bit=11) == 1: + self.set_status_bit(bit=4, value=True) + self.set_status_bit(bit=13, value=True) + self.set_status_bit(bit=14, value=False) + self.set_status_bit(bit=12, value=False) + self.set_status_bit(bit=17, value=False) # Move to absolute position if self.get_control_bit(13) == 1: self.move( diff --git a/schunk_egu_egk_gripper_dummy/tests/test_dummy.py b/schunk_egu_egk_gripper_dummy/tests/test_dummy.py index a06c1d0..a3a0987 100644 --- a/schunk_egu_egk_gripper_dummy/tests/test_dummy.py +++ b/schunk_egu_egk_gripper_dummy/tests/test_dummy.py @@ -128,3 +128,14 @@ def test_dummy_supports_prepare_for_shutdown(): dummy.set_control_bit(bit=3, value=True) dummy.process_control_bits() assert dummy.get_status_bit(bit=2) == 1 # ready for shutdown + + +def test_dummy_supports_release_workpiece(): + dummy = Dummy() + dummy.set_control_bit(bit=11, value=True) + dummy.process_control_bits() + assert dummy.get_status_bit(bit=4) == 1 # command successfully processed + assert dummy.get_status_bit(bit=13) == 1 # position reached + assert dummy.get_status_bit(bit=14) == 0 # pre-grip started + assert dummy.get_status_bit(bit=12) == 0 # workpiece gripped + assert dummy.get_status_bit(bit=17) == 0 # wrong workpiece gripped diff --git a/schunk_egu_egk_gripper_tests/test/helpers.py b/schunk_egu_egk_gripper_tests/test/helpers.py index 8096a8b..2a950ff 100644 --- a/schunk_egu_egk_gripper_tests/test/helpers.py +++ b/schunk_egu_egk_gripper_tests/test/helpers.py @@ -5,6 +5,7 @@ import uuid from rclpy.executors import MultiThreadedExecutor import time +from rclpy.action import ActionClient class TopicGetsPublished(Node): @@ -53,6 +54,31 @@ def spin(self) -> None: self.event.set() +class ActionReturnsResult(Node): + def __init__(self, action: str, type: Any, goal: Any): + self.node_name = "call_action" + str(uuid.uuid4()).replace("-", "") + super().__init__(self.node_name) + self.event = Event() + + client = ActionClient(self, type, action) + client.wait_for_server(1.0) + + self.goal_future = client.send_goal_async(goal) + self.result = None + self.thread = Thread(target=self.spin) + self.thread.start() + + def spin(self) -> None: + executor = MultiThreadedExecutor() + executor.add_node(self) + executor.spin_until_future_complete(self.goal_future) + goal_handle = self.goal_future.result() + result_future = goal_handle.get_result_async() + executor.spin_until_future_complete(result_future) + self.result = result_future.result().result + self.event.set() + + def check_each_in(elements: list, node_method: str) -> None: node = Node("test") until_ready = 2.0 # sec diff --git a/schunk_egu_egk_gripper_tests/test/test_actions.py b/schunk_egu_egk_gripper_tests/test/test_actions.py index 8c8a82f..b80f2e8 100644 --- a/schunk_egu_egk_gripper_tests/test/test_actions.py +++ b/schunk_egu_egk_gripper_tests/test/test_actions.py @@ -1,11 +1,9 @@ import pytest from test.conftest import launch_description -from test.helpers import check_each_in -import rclpy -from rclpy.node import Node -from rclpy.action import ActionClient +from test.helpers import check_each_in, ActionReturnsResult from schunk_egu_egk_gripper_interfaces.action import ( # type: ignore[attr-defined] MoveToAbsolutePosition, + ReleaseWorkpiece, ) @@ -25,17 +23,17 @@ def test_driver_advertices_all_relevant_actions(running_driver): @pytest.mark.launch(fixture=launch_description) def test_driver_moves_to_absolute_position(running_driver): - node = Node("move_test") - - client = ActionClient(node, MoveToAbsolutePosition, "/move_to_absolute_position") - client.wait_for_server() goal = MoveToAbsolutePosition.Goal() + action = ActionReturnsResult( + "/move_to_absolute_position", MoveToAbsolutePosition, goal + ) + action.event.wait(timeout=1) + assert action.result.position_reached - goal_future = client.send_goal_async(goal) - rclpy.spin_until_future_complete(node, goal_future) - goal_handle = goal_future.result() - result_future = goal_handle.get_result_async() - rclpy.spin_until_future_complete(node, result_future) - result = result_future.result().result - assert result.position_reached +@pytest.mark.launch(fixture=launch_description) +def test_driver_releases_workpieces(running_driver): + goal = ReleaseWorkpiece.Goal() + action = ActionReturnsResult("/release_workpiece", ReleaseWorkpiece, goal) + action.event.wait(timeout=1) + assert action.result.released_workpiece From 1b495cded4d1c34b67a7fd1b7dbc3f93dbb55e33 Mon Sep 17 00:00:00 2001 From: Stefan Scherzinger Date: Tue, 13 Aug 2024 18:27:22 +0200 Subject: [PATCH 16/25] Use the system uptime for the _softreset_ service This is easier to implement and test with the dummy. --- .../src/schunk_gripper_wrapper.cpp | 59 +++++++------------ .../schunk_egu_egk_gripper_dummy/main.py | 1 + schunk_egu_egk_gripper_dummy/src/dummy.py | 16 +++++ .../tests/test_dummy.py | 17 ++++++ .../tests/test_plc_communication.py | 7 +++ .../communication.hpp | 2 + .../test/test_services.py | 8 +++ 7 files changed, 71 insertions(+), 39 deletions(-) diff --git a/schunk_egu_egk_gripper_driver/src/schunk_gripper_wrapper.cpp b/schunk_egu_egk_gripper_driver/src/schunk_gripper_wrapper.cpp index 0801f57..6dd8db6 100644 --- a/schunk_egu_egk_gripper_driver/src/schunk_gripper_wrapper.cpp +++ b/schunk_egu_egk_gripper_driver/src/schunk_gripper_wrapper.cpp @@ -1300,59 +1300,40 @@ void SchunkGripperNode::fast_stop_srv(const std::shared_ptr, void SchunkGripperNode::softreset_srv(const std::shared_ptr, std::shared_ptr res) { std::unique_lock lock(lock_mutex, std::defer_lock); + getParameter(SYSTEM_UPTIME_INST, 1, UINT32_DATA); + const auto uptime_before_reset = uint32_vector[0]; + auto uptime = uptime_before_reset; + + // Detect soft resets by checking the system uptime. + auto system_reset = [&](){ + try { + getParameter(SYSTEM_UPTIME_INST, 1, UINT32_DATA); + uptime = uint32_vector[0]; + } + catch(const char* error){} + return (uptime < uptime_before_reset) ? true : false; + }; + try { RCLCPP_INFO(this->get_logger(),"SOFTRESET"); - std::unique_lock lock_service(lock_service_post); - set_command = SOFT_RESET; - - sendService(lock); lock_service.unlock(); } - catch(const char* res){} - - bool connection_once_lost = false; - - if(handshake != gripperBitInput(COMMAND_RECEIVED_TOGGLE)) - { - for(int i = 0; rclcpp::ok() && i <= 1000 && check(); i++) - { - try - { - runGets(); //if connection is back, don't catch - if(connection_once_lost == true) - { - connection_error = "OK"; - break; - } - } - catch(const char* res) - { - connection_error = res; - std::chrono::milliseconds sleep_time(10); - std::this_thread::sleep_for(sleep_time); - connection_once_lost = true; - } - } + catch(const char* error){ + RCLCPP_INFO(this->get_logger(), "Softreset failed!"); + res->success = false; } - else RCLCPP_WARN(this->get_logger(), "Handshake failed!"); - if(connection_once_lost == true && rclcpp::ok() && connection_error == "OK") - { - RCLCPP_INFO(this->get_logger(), "Softreset succeeded!"); - res->success = true; - } - else + while (!system_reset()) { - RCLCPP_INFO(this->get_logger(), "Softreset failed!"); - res->success = false; + runGets(); } + res->success = true; lock.unlock(); gripper_updater->force_update(); - } //Prepare for shutdown service callback void SchunkGripperNode::prepare_for_shutdown_srv(const std::shared_ptr, std::shared_ptr res) diff --git a/schunk_egu_egk_gripper_dummy/schunk_egu_egk_gripper_dummy/main.py b/schunk_egu_egk_gripper_dummy/schunk_egu_egk_gripper_dummy/main.py index 46ec41f..ac3a06d 100644 --- a/schunk_egu_egk_gripper_dummy/schunk_egu_egk_gripper_dummy/main.py +++ b/schunk_egu_egk_gripper_dummy/schunk_egu_egk_gripper_dummy/main.py @@ -7,6 +7,7 @@ # Components dummy = Dummy() +dummy.start() server = FastAPI() client = ["http://localhost:8001"] diff --git a/schunk_egu_egk_gripper_dummy/src/dummy.py b/schunk_egu_egk_gripper_dummy/src/dummy.py index 88615ae..54d9817 100644 --- a/schunk_egu_egk_gripper_dummy/src/dummy.py +++ b/schunk_egu_egk_gripper_dummy/src/dummy.py @@ -38,6 +38,7 @@ def sample(self, t: float) -> Tuple[float, float]: class Dummy(object): def __init__(self): + self.starttime = time.time() self.thread = Thread(target=self._run) self.running = False self.done = False @@ -48,6 +49,7 @@ def __init__(self): self.plc_output = "0x0048" self.actual_position = "0x0230" self.actual_speed = "0x0238" + self.system_uptime = "0x1400" self.error_byte = 12 self.diagnostics_byte = 15 self.valid_status_bits = list(range(0, 10)) + [11, 12, 13, 14, 16, 17, 31] @@ -87,6 +89,8 @@ def stop(self) -> None: def _run(self) -> None: while not self.done: + elapsed = time.time() - self.starttime + self.set_system_uptime(int(elapsed)) time.sleep(1) print("Done") @@ -257,6 +261,13 @@ def get_actual_speed(self) -> float: read_speed = self.data[self.actual_speed][0] return struct.unpack("f", bytes.fromhex(read_speed))[0] + def set_system_uptime(self, uptime: int) -> None: + self.data[self.system_uptime] = [bytes(struct.pack("i", uptime)).hex().upper()] + + def get_system_uptime(self) -> int: + uptime = self.data[self.system_uptime][0] + return struct.unpack("i", bytes.fromhex(uptime))[0] + def process_control_bits(self) -> None: """ See the gripper's firmware documentation for EtherNet/IP [1]: @@ -304,6 +315,11 @@ def process_control_bits(self) -> None: self.set_status_bit(bit=14, value=False) self.set_status_bit(bit=12, value=False) self.set_status_bit(bit=17, value=False) + + # Soft reset + if self.get_control_bit(bit=4) == 1: + self.set_system_uptime(0) + # Move to absolute position if self.get_control_bit(13) == 1: self.move( diff --git a/schunk_egu_egk_gripper_dummy/tests/test_dummy.py b/schunk_egu_egk_gripper_dummy/tests/test_dummy.py index a3a0987..de77c5c 100644 --- a/schunk_egu_egk_gripper_dummy/tests/test_dummy.py +++ b/schunk_egu_egk_gripper_dummy/tests/test_dummy.py @@ -1,6 +1,7 @@ from src.dummy import Dummy import pytest import struct +import time # [1]: https://stb.cloud.schunk.com/media/IM0046706.PDF @@ -139,3 +140,19 @@ def test_dummy_supports_release_workpiece(): assert dummy.get_status_bit(bit=14) == 0 # pre-grip started assert dummy.get_status_bit(bit=12) == 0 # workpiece gripped assert dummy.get_status_bit(bit=17) == 0 # wrong workpiece gripped + + +def test_dummy_supports_softreset(): + dummy = Dummy() + dummy.start() # fake some system uptime + initial = dummy.get_system_uptime() + time.sleep(1.5) + later = dummy.get_system_uptime() + time.sleep(1.5) + dummy.set_control_bit(bit=4, value=True) + dummy.process_control_bits() + after_reset = dummy.get_system_uptime() + dummy.stop() + + assert initial < later + assert after_reset < later # restart resets the uptime diff --git a/schunk_egu_egk_gripper_dummy/tests/test_plc_communication.py b/schunk_egu_egk_gripper_dummy/tests/test_plc_communication.py index f147afe..9cdb6bd 100644 --- a/schunk_egu_egk_gripper_dummy/tests/test_plc_communication.py +++ b/schunk_egu_egk_gripper_dummy/tests/test_plc_communication.py @@ -165,3 +165,10 @@ def test_dummy_supports_reading_actual_speed(): speed = 66.5 dummy.set_actual_speed(speed) assert pytest.approx(dummy.get_actual_speed()) == speed + + +def test_dummy_supports_reading_and_writing_system_uptime(): + dummy = Dummy() + uptime = 1234 # secs + dummy.set_system_uptime(uptime) + assert dummy.get_system_uptime() == uptime diff --git a/schunk_egu_egk_gripper_library/include/schunk_egu_egk_gripper_library/communication.hpp b/schunk_egu_egk_gripper_library/include/schunk_egu_egk_gripper_library/communication.hpp index 469d725..8762199 100644 --- a/schunk_egu_egk_gripper_library/include/schunk_egu_egk_gripper_library/communication.hpp +++ b/schunk_egu_egk_gripper_library/include/schunk_egu_egk_gripper_library/communication.hpp @@ -113,6 +113,8 @@ #define SW_BUILD_DATE_INST "0x1100" #define SW_BUILD_TIME_INST "0x1108" #define SW_VERSION_TXT_INST "0x1118" +#define SYSTEM_UPTIME_INST "0x1400" + //Datatypes in JSON #define BOOL_DATA 0 #define INT32_DATA 3 diff --git a/schunk_egu_egk_gripper_tests/test/test_services.py b/schunk_egu_egk_gripper_tests/test/test_services.py index fbe3b8e..978ce49 100644 --- a/schunk_egu_egk_gripper_tests/test/test_services.py +++ b/schunk_egu_egk_gripper_tests/test/test_services.py @@ -9,6 +9,7 @@ ReleaseForManualMovement, GripperInfo, PrepareForShutdown, + Softreset, ) from test.helpers import ServiceReturnsResult @@ -77,6 +78,13 @@ def test_driver_supports_release_for_manual_movement(running_driver): assert service.result.success is True +@pytest.mark.launch(fixture=launch_description) +def test_driver_supports_softreset(running_driver): + service = ServiceReturnsResult("/softreset", Softreset, Softreset.Request()) + service.event.wait(timeout=1) + assert service.result.success is True + + @pytest.mark.launch(fixture=launch_description) def test_driver_supports_stop(running_driver): service = ServiceReturnsResult("/stop", Stop, Stop.Request()) From d12526721ff31894015a73c9d3dc09c083b007e5 Mon Sep 17 00:00:00 2001 From: Stefan Scherzinger Date: Thu, 15 Aug 2024 18:26:49 +0200 Subject: [PATCH 17/25] Fix the computation of actual position and speed We now return the actual data, not the metadata. Also, we now interpret target position and target speed in the plc commands as unsigned integers (position in um, speed in um/s) as stated in the gripper's TCP/IP manual. Only the data in `self.data` need to be stored and returned as floats. Check this manual for further details: [1]: https://stb.cloud.schunk.com/media/IM0046706.PDF --- schunk_egu_egk_gripper_dummy/config/data.json | 5 +++- .../config/system_parameter_codes | 1 + schunk_egu_egk_gripper_dummy/src/dummy.py | 13 +++++++--- .../tests/test_dummy.py | 26 +++++++++++++++---- .../tests/test_plc_communication.py | 12 ++++----- .../tests/test_requests.py | 14 +++++++++- .../test/test_actions.py | 3 +++ 7 files changed, 57 insertions(+), 17 deletions(-) diff --git a/schunk_egu_egk_gripper_dummy/config/data.json b/schunk_egu_egk_gripper_dummy/config/data.json index 371216e..470c567 100644 --- a/schunk_egu_egk_gripper_dummy/config/data.json +++ b/schunk_egu_egk_gripper_dummy/config/data.json @@ -24,11 +24,14 @@ "00000000" ], "0x0230": [ - "42263D87" + "00000000" ], "0x0238": [ "00000000" ], + "0x0240": [ + "3C56B24F" + ], "0x0380": [ "0000" ], diff --git a/schunk_egu_egk_gripper_dummy/config/system_parameter_codes b/schunk_egu_egk_gripper_dummy/config/system_parameter_codes index 0b38c21..2464603 100644 --- a/schunk_egu_egk_gripper_dummy/config/system_parameter_codes +++ b/schunk_egu_egk_gripper_dummy/config/system_parameter_codes @@ -13,6 +13,7 @@ 0x0210 0x0230 0x0238 +0x0240 0x0380 0x03A8 0x03B0 diff --git a/schunk_egu_egk_gripper_dummy/src/dummy.py b/schunk_egu_egk_gripper_dummy/src/dummy.py index 54d9817..77be98b 100644 --- a/schunk_egu_egk_gripper_dummy/src/dummy.py +++ b/schunk_egu_egk_gripper_dummy/src/dummy.py @@ -144,7 +144,10 @@ def get_data(self, query: dict[str, str]) -> list: if offset + count >= len(self.metadata): return result for i in range(count): - result.append(self.metadata[offset + i]) + id = self.metadata[offset + i]["instance"] + inst = hex(id)[2:].upper().zfill(4) + inst = "0x" + inst + result.append(self.data[inst][0]) return result if "inst" in query and "count" in query: @@ -240,10 +243,12 @@ def set_control_bit(self, bit: int, value: bool) -> bool: return True def get_target_position(self) -> float: - return struct.unpack("f", self.plc_output_buffer[4:8])[0] + return struct.unpack("i", self.plc_output_buffer[4:8])[0] / 1000.0 # um to mm def get_target_speed(self) -> float: - return struct.unpack("f", self.plc_output_buffer[8:12])[0] + return ( + struct.unpack("i", self.plc_output_buffer[8:12])[0] / 1000.0 + ) # um/s to mm/s def set_actual_position(self, position: float) -> None: self.data[self.actual_position] = [ @@ -321,7 +326,7 @@ def process_control_bits(self) -> None: self.set_system_uptime(0) # Move to absolute position - if self.get_control_bit(13) == 1: + if self.get_control_bit(bit=13) == 1: self.move( target_pos=self.get_target_position(), target_speed=self.get_target_speed(), diff --git a/schunk_egu_egk_gripper_dummy/tests/test_dummy.py b/schunk_egu_egk_gripper_dummy/tests/test_dummy.py index de77c5c..15c1477 100644 --- a/schunk_egu_egk_gripper_dummy/tests/test_dummy.py +++ b/schunk_egu_egk_gripper_dummy/tests/test_dummy.py @@ -63,11 +63,11 @@ def test_dummy_always_toggles_command_received_bit(): def test_dummy_moves_to_absolute_position(): dummy = Dummy() - target_pos = 12.345 - target_speed = 50.3 + target_pos = 12345 # mu + target_speed = 50300 # mu / s control_double_word = "00200000" # bit 13 - set_position = bytes(struct.pack("f", target_pos)).hex().upper() - set_speed = bytes(struct.pack("f", target_speed)).hex().upper() + set_position = bytes(struct.pack("i", target_pos)).hex().upper() + set_speed = bytes(struct.pack("i", target_speed)).hex().upper() gripping_force = "00000000" command = { "inst": dummy.plc_output, @@ -78,11 +78,27 @@ def test_dummy_moves_to_absolute_position(): dummy.post(command) # Done - assert pytest.approx(dummy.get_actual_position()) == target_pos + assert dummy.get_actual_position() == pytest.approx(target_pos / 1000.0) assert dummy.get_status_bit(bit=13) == 1 # position reached assert dummy.get_status_bit(bit=4) == 1 # command successfully processed +def test_dummy_updates_internal_state_when_moving(): + dummy = Dummy() + query = {"offset": 15, "count": 3} # actual position, speed, and current + before = dummy.get_data(query) + + # Move + target_pos = 10.34 + target_speed = 15.0 + assert pytest.approx(dummy.get_actual_position()) != target_pos + dummy.move(target_pos=target_pos, target_speed=target_speed) + assert pytest.approx(dummy.get_actual_position()) == target_pos + + after = dummy.get_data(query) + assert before != after + + def test_dummy_performs_break_test(): dummy = Dummy() dummy.set_control_bit(bit=30, value=True) diff --git a/schunk_egu_egk_gripper_dummy/tests/test_plc_communication.py b/schunk_egu_egk_gripper_dummy/tests/test_plc_communication.py index 9cdb6bd..574e9b4 100644 --- a/schunk_egu_egk_gripper_dummy/tests/test_plc_communication.py +++ b/schunk_egu_egk_gripper_dummy/tests/test_plc_communication.py @@ -123,16 +123,16 @@ def test_dummy_rejects_toggling_reserved_status_bits(): def test_dummy_supports_reading_target_position(): dummy = Dummy() - target_pos = 0.0123 - dummy.plc_output_buffer[4:8] = bytes(struct.pack("f", target_pos)) - assert pytest.approx(dummy.get_target_position()) == target_pos + target_pos = 12300 # um + dummy.plc_output_buffer[4:8] = bytes(struct.pack("i", target_pos)) + assert pytest.approx(dummy.get_target_position()) == target_pos / 1000.0 def test_dummy_supports_reading_target_speed(): dummy = Dummy() - target_speed = 55.3 - dummy.plc_output_buffer[8:12] = bytes(struct.pack("f", target_speed)) - assert pytest.approx(dummy.get_target_speed()) == target_speed + target_speed = 55300 + dummy.plc_output_buffer[8:12] = bytes(struct.pack("i", target_speed)) + assert pytest.approx(dummy.get_target_speed()) == target_speed / 1000.0 def test_dummy_supports_writing_actual_position(): diff --git a/schunk_egu_egk_gripper_dummy/tests/test_requests.py b/schunk_egu_egk_gripper_dummy/tests/test_requests.py index 48667c3..2d96fd8 100644 --- a/schunk_egu_egk_gripper_dummy/tests/test_requests.py +++ b/schunk_egu_egk_gripper_dummy/tests/test_requests.py @@ -30,7 +30,19 @@ def test_dummy_survives_invalid_enum_requests(): def test_dummy_responds_correctly_to_data_offset_requests(): dummy = Dummy() query = {"offset": 15, "count": 3} - expected = [dummy.metadata[15], dummy.metadata[16], dummy.metadata[17]] + + def get_hex(number: int) -> str: + plain = hex(number)[2:].upper().zfill(4) + return "0x" + plain + + inst1 = get_hex(dummy.metadata[15]["instance"]) + inst2 = get_hex(dummy.metadata[16]["instance"]) + inst3 = get_hex(dummy.metadata[17]["instance"]) + data1 = dummy.data[inst1][0] + data2 = dummy.data[inst2][0] + data3 = dummy.data[inst3][0] + + expected = [data1, data2, data3] assert dummy.get_data(query) == expected diff --git a/schunk_egu_egk_gripper_tests/test/test_actions.py b/schunk_egu_egk_gripper_tests/test/test_actions.py index b80f2e8..11869f0 100644 --- a/schunk_egu_egk_gripper_tests/test/test_actions.py +++ b/schunk_egu_egk_gripper_tests/test/test_actions.py @@ -24,11 +24,14 @@ def test_driver_advertices_all_relevant_actions(running_driver): @pytest.mark.launch(fixture=launch_description) def test_driver_moves_to_absolute_position(running_driver): goal = MoveToAbsolutePosition.Goal() + goal.absolute_position = 43.55 + goal.velocity_of_movement = 55.66 action = ActionReturnsResult( "/move_to_absolute_position", MoveToAbsolutePosition, goal ) action.event.wait(timeout=1) assert action.result.position_reached + assert pytest.approx(action.result.absolute_position) == goal.absolute_position @pytest.mark.launch(fixture=launch_description) From 1bbbb90e28c40a87f1836a7317c6eaf9674085cb Mon Sep 17 00:00:00 2001 From: Stefan Scherzinger Date: Fri, 16 Aug 2024 12:15:34 +0200 Subject: [PATCH 18/25] Implement the dummy's support for relative positioning Also: - Fix the `MotionProfile`s finish condition. - Make tests for absolute positioning more watertight. Running several moves in a row was crucial in spotting the erroneous finish condition. - Remove specific timeouts in the ROS2 action tests. It's more robust this way. If they fail, the CI fails with a reason. - Make sure that the driver resets relevant status bits from previous requests. --- schunk_egu_egk_gripper_dummy/src/dummy.py | 21 ++++++++++- .../tests/test_dummy.py | 35 ++++++++++++++---- .../tests/test_requests.py | 11 ++++++ .../test/test_actions.py | 37 +++++++++++++++---- 4 files changed, 87 insertions(+), 17 deletions(-) diff --git a/schunk_egu_egk_gripper_dummy/src/dummy.py b/schunk_egu_egk_gripper_dummy/src/dummy.py index 77be98b..f822e72 100644 --- a/schunk_egu_egk_gripper_dummy/src/dummy.py +++ b/schunk_egu_egk_gripper_dummy/src/dummy.py @@ -75,6 +75,7 @@ def __init__(self): self.plc_input_buffer = bytearray(bytes.fromhex(self.data[self.plc_input][0])) self.plc_output_buffer = bytearray(bytes.fromhex(self.data[self.plc_output][0])) + self.initial_state = [self.plc_input_buffer.hex().upper()] def start(self) -> None: if self.running: @@ -103,7 +104,7 @@ def move(self, target_pos: float, target_speed: float) -> None: ) start = time.time() actual_pos, actual_speed = motion.sample(0) - while abs(actual_pos) < abs(target_pos): + while abs(target_pos - actual_pos) > 0.001: # mm t = time.time() - start actual_pos, actual_speed = motion.sample(t) self.set_actual_position(actual_pos) @@ -279,6 +280,10 @@ def process_control_bits(self) -> None: https://stb.cloud.schunk.com/media/IM0046706.PDF """ + # Reset success of previous commands + self.set_status_bit(bit=4, value=False) + self.set_status_bit(bit=8, value=False) + self.set_status_bit(bit=13, value=False) # Command received toggle self.toggle_status_bit(bit=5) @@ -324,6 +329,7 @@ def process_control_bits(self) -> None: # Soft reset if self.get_control_bit(bit=4) == 1: self.set_system_uptime(0) + self.data[self.plc_input] = self.initial_state # Move to absolute position if self.get_control_bit(bit=13) == 1: @@ -333,3 +339,16 @@ def process_control_bits(self) -> None: ) self.set_status_bit(bit=13, value=True) self.set_status_bit(bit=4, value=True) + + # Move to relative position + if self.get_control_bit(bit=14) == 1: + target_pos = ( + self.get_actual_position() + + self.get_target_position() # interpret relative + ) + self.move( + target_pos=target_pos, + target_speed=self.get_target_speed(), + ) + self.set_status_bit(bit=13, value=True) + self.set_status_bit(bit=4, value=True) diff --git a/schunk_egu_egk_gripper_dummy/tests/test_dummy.py b/schunk_egu_egk_gripper_dummy/tests/test_dummy.py index 15c1477..7ead3df 100644 --- a/schunk_egu_egk_gripper_dummy/tests/test_dummy.py +++ b/schunk_egu_egk_gripper_dummy/tests/test_dummy.py @@ -63,9 +63,29 @@ def test_dummy_always_toggles_command_received_bit(): def test_dummy_moves_to_absolute_position(): dummy = Dummy() - target_pos = 12345 # mu - target_speed = 50300 # mu / s - control_double_word = "00200000" # bit 13 + target_positions = [12345, 10555, 77000, 1500] # mu + target_speeds = [50300, 40000, 10500, 20999] # mu / s + for target_pos, target_speed in zip(target_positions, target_speeds): + control_double_word = "00200000" # bit 13 + set_position = bytes(struct.pack("i", target_pos)).hex().upper() + set_speed = bytes(struct.pack("i", target_speed)).hex().upper() + gripping_force = "00000000" + command = { + "inst": dummy.plc_output, + "value": control_double_word + set_position + set_speed + gripping_force, + } + + dummy.post(command) + assert dummy.get_actual_position() == pytest.approx(target_pos / 1000.0) + assert dummy.get_status_bit(bit=13) == 1 # position reached + assert dummy.get_status_bit(bit=4) == 1 # command successfully processed + + +def test_dummy_moves_to_relative_position(): + dummy = Dummy() + target_pos = -5000 # mu + target_speed = 12000 # mu / s + control_double_word = "00400000" # bit 14 set_position = bytes(struct.pack("i", target_pos)).hex().upper() set_speed = bytes(struct.pack("i", target_speed)).hex().upper() gripping_force = "00000000" @@ -73,12 +93,11 @@ def test_dummy_moves_to_absolute_position(): "inst": dummy.plc_output, "value": control_double_word + set_position + set_speed + gripping_force, } - - # Motion + before = dummy.get_actual_position() dummy.post(command) - - # Done - assert dummy.get_actual_position() == pytest.approx(target_pos / 1000.0) + after = dummy.get_actual_position() + assert after < before # we are decreasing + assert after == pytest.approx(before + target_pos / 1000.0) assert dummy.get_status_bit(bit=13) == 1 # position reached assert dummy.get_status_bit(bit=4) == 1 # command successfully processed diff --git a/schunk_egu_egk_gripper_dummy/tests/test_requests.py b/schunk_egu_egk_gripper_dummy/tests/test_requests.py index 2d96fd8..35e185f 100644 --- a/schunk_egu_egk_gripper_dummy/tests/test_requests.py +++ b/schunk_egu_egk_gripper_dummy/tests/test_requests.py @@ -110,3 +110,14 @@ def test_dummy_rejects_invalid_post_requests(): invalid_inst = "0x9999" data = {"inst": invalid_inst, "value": valid_data} assert client.post("/adi/update.json", data=data).json() == {"result": 1} + + +def test_dummy_resets_success_status_bits_with_new_post_requests(): + dummy = Dummy() + dummy.set_status_bit(bit=13, value=True) # position reached + dummy.set_status_bit(bit=4, value=True) # command successful + empty_command = "01" + "".zfill(30) # only fast stop active + data = {"inst": dummy.plc_output, "value": empty_command} + dummy.post(data) + assert dummy.get_status_bit(bit=13) == 0 + assert dummy.get_status_bit(bit=4) == 0 diff --git a/schunk_egu_egk_gripper_tests/test/test_actions.py b/schunk_egu_egk_gripper_tests/test/test_actions.py index 11869f0..a4003c5 100644 --- a/schunk_egu_egk_gripper_tests/test/test_actions.py +++ b/schunk_egu_egk_gripper_tests/test/test_actions.py @@ -1,8 +1,9 @@ import pytest from test.conftest import launch_description -from test.helpers import check_each_in, ActionReturnsResult +from test.helpers import check_each_in, ActionReturnsResult, get_current_state from schunk_egu_egk_gripper_interfaces.action import ( # type: ignore[attr-defined] MoveToAbsolutePosition, + MoveToRelativePosition, ReleaseWorkpiece, ) @@ -23,20 +24,40 @@ def test_driver_advertices_all_relevant_actions(running_driver): @pytest.mark.launch(fixture=launch_description) def test_driver_moves_to_absolute_position(running_driver): - goal = MoveToAbsolutePosition.Goal() - goal.absolute_position = 43.55 - goal.velocity_of_movement = 55.66 + test_positions = [43.55, 17.02, 38.55, 103.7] + test_speeds = [55.66, 10.5, 40.0, 88.8] + for test_pos, test_speed in zip(test_positions, test_speeds): + goal = MoveToAbsolutePosition.Goal() + goal.absolute_position = test_pos + goal.velocity_of_movement = test_speed + action = ActionReturnsResult( + "/move_to_absolute_position", MoveToAbsolutePosition, goal + ) + action.event.wait() + assert action.result.position_reached + assert pytest.approx(action.result.absolute_position) == goal.absolute_position + + +@pytest.mark.launch(fixture=launch_description) +def test_driver_moves_to_relative_position(running_driver): + initial_pos = get_current_state(variable="actual_position") + goal = MoveToRelativePosition.Goal() + goal.signed_relative_position = -10.03 + goal.velocity_of_movement = 15.0 action = ActionReturnsResult( - "/move_to_absolute_position", MoveToAbsolutePosition, goal + "/move_to_relative_position", MoveToRelativePosition, goal ) - action.event.wait(timeout=1) + action.event.wait() assert action.result.position_reached - assert pytest.approx(action.result.absolute_position) == goal.absolute_position + assert ( + pytest.approx(action.result.absolute_position) + == initial_pos + goal.signed_relative_position + ) @pytest.mark.launch(fixture=launch_description) def test_driver_releases_workpieces(running_driver): goal = ReleaseWorkpiece.Goal() action = ActionReturnsResult("/release_workpiece", ReleaseWorkpiece, goal) - action.event.wait(timeout=1) + action.event.wait() assert action.result.released_workpiece From 0af6ef29625a84ee9aeff17789df38ac55c431b0 Mon Sep 17 00:00:00 2001 From: Stefan Scherzinger Date: Fri, 16 Aug 2024 14:46:14 +0200 Subject: [PATCH 19/25] Cleanup background threads after dummy tests finish --- schunk_egu_egk_gripper_dummy/tests/test_requests.py | 4 +++- schunk_egu_egk_gripper_dummy/tests/test_webserver.py | 6 +++++- 2 files changed, 8 insertions(+), 2 deletions(-) diff --git a/schunk_egu_egk_gripper_dummy/tests/test_requests.py b/schunk_egu_egk_gripper_dummy/tests/test_requests.py index 35e185f..8677e72 100644 --- a/schunk_egu_egk_gripper_dummy/tests/test_requests.py +++ b/schunk_egu_egk_gripper_dummy/tests/test_requests.py @@ -1,5 +1,5 @@ from src.dummy import Dummy -from schunk_egu_egk_gripper_dummy.main import server +from schunk_egu_egk_gripper_dummy.main import server, dummy as client_dummy from fastapi.testclient import TestClient @@ -95,6 +95,7 @@ def test_dummy_stores_post_requests(): def test_dummy_rejects_invalid_post_requests(): client = TestClient(server) + client_dummy.start() valid_data = "AABBCCDD" valid_inst = "0x0238" @@ -110,6 +111,7 @@ def test_dummy_rejects_invalid_post_requests(): invalid_inst = "0x9999" data = {"inst": invalid_inst, "value": valid_data} assert client.post("/adi/update.json", data=data).json() == {"result": 1} + client_dummy.stop() def test_dummy_resets_success_status_bits_with_new_post_requests(): diff --git a/schunk_egu_egk_gripper_dummy/tests/test_webserver.py b/schunk_egu_egk_gripper_dummy/tests/test_webserver.py index 91444c1..1a22094 100644 --- a/schunk_egu_egk_gripper_dummy/tests/test_webserver.py +++ b/schunk_egu_egk_gripper_dummy/tests/test_webserver.py @@ -1,23 +1,27 @@ -from schunk_egu_egk_gripper_dummy.main import server +from schunk_egu_egk_gripper_dummy.main import server, dummy from fastapi.testclient import TestClient def test_info_route_is_available(): client = TestClient(server) assert client.get("/adi/info.json").is_success + dummy.stop() def test_enum_route_is_available(): client = TestClient(server) assert client.get("/adi/enum.json").is_success + dummy.stop() def test_data_route_is_available(): client = TestClient(server) assert client.get("/adi/data.json").is_success + dummy.stop() def test_update_route_is_available(): client = TestClient(server) data = {"inst": 0, "value": "0"} assert client.post("/adi/update.json", data=data).is_success + dummy.stop() From 67ad2d4a2089c719094dcc132591f38dc3dbe647 Mon Sep 17 00:00:00 2001 From: Stefan Scherzinger Date: Fri, 16 Aug 2024 14:59:02 +0200 Subject: [PATCH 20/25] Add a convenience script for starting the dummy --- schunk_egu_egk_gripper_dummy/README.md | 2 +- schunk_egu_egk_gripper_dummy/start_dummy.sh | 1 + 2 files changed, 2 insertions(+), 1 deletion(-) create mode 100755 schunk_egu_egk_gripper_dummy/start_dummy.sh diff --git a/schunk_egu_egk_gripper_dummy/README.md b/schunk_egu_egk_gripper_dummy/README.md index 66e8d24..82e2346 100644 --- a/schunk_egu_egk_gripper_dummy/README.md +++ b/schunk_egu_egk_gripper_dummy/README.md @@ -10,7 +10,7 @@ pip install --user fastapi uvicorn ## Getting started 1. Start the dummy standalone with ```bash - uvicorn schunk_egu_egk_gripper_dummy.main:server --port 8000 --reload + ./start_dummy.sh ``` ## Run tests locally diff --git a/schunk_egu_egk_gripper_dummy/start_dummy.sh b/schunk_egu_egk_gripper_dummy/start_dummy.sh new file mode 100755 index 0000000..1551d71 --- /dev/null +++ b/schunk_egu_egk_gripper_dummy/start_dummy.sh @@ -0,0 +1 @@ +uvicorn schunk_egu_egk_gripper_dummy.main:server --port 8000 --reload From 6f28fc132f54b3efe672603e5e0aa6e74d864260 Mon Sep 17 00:00:00 2001 From: Stefan Scherzinger Date: Fri, 23 Aug 2024 16:53:09 +0200 Subject: [PATCH 21/25] Add a test for the driver's `/grip` action The _EGU_ and _EGK_ versions differ in the action goal type. Let's see if we can mimic both at some point. --- schunk_egu_egk_gripper_dummy/src/dummy.py | 8 ++++++++ schunk_egu_egk_gripper_dummy/tests/test_dummy.py | 10 ++++++++++ .../tests/test_requests.py | 2 ++ .../test/test_actions.py | 16 ++++++++++++++++ 4 files changed, 36 insertions(+) diff --git a/schunk_egu_egk_gripper_dummy/src/dummy.py b/schunk_egu_egk_gripper_dummy/src/dummy.py index f822e72..f766f51 100644 --- a/schunk_egu_egk_gripper_dummy/src/dummy.py +++ b/schunk_egu_egk_gripper_dummy/src/dummy.py @@ -283,6 +283,7 @@ def process_control_bits(self) -> None: # Reset success of previous commands self.set_status_bit(bit=4, value=False) self.set_status_bit(bit=8, value=False) + self.set_status_bit(bit=12, value=False) self.set_status_bit(bit=13, value=False) # Command received toggle @@ -352,3 +353,10 @@ def process_control_bits(self) -> None: ) self.set_status_bit(bit=13, value=True) self.set_status_bit(bit=4, value=True) + + # Grip workpiece + if self.get_control_bit(bit=12) == 1: + self.set_status_bit(bit=12, value=True) + self.set_status_bit(bit=4, value=True) + self.set_status_bit(bit=11, value=False) + self.set_status_bit(bit=16, value=False) diff --git a/schunk_egu_egk_gripper_dummy/tests/test_dummy.py b/schunk_egu_egk_gripper_dummy/tests/test_dummy.py index 7ead3df..1646d82 100644 --- a/schunk_egu_egk_gripper_dummy/tests/test_dummy.py +++ b/schunk_egu_egk_gripper_dummy/tests/test_dummy.py @@ -191,3 +191,13 @@ def test_dummy_supports_softreset(): assert initial < later assert after_reset < later # restart resets the uptime + + +def test_dummy_supports_grip(): + dummy = Dummy() + dummy.set_control_bit(bit=12, value=True) # grip workpiece + dummy.process_control_bits() + assert dummy.get_status_bit(bit=12) == 1 # workpiece gripped + assert dummy.get_status_bit(bit=4) == 1 # command successfully processed + assert dummy.get_status_bit(bit=16) == 0 # workpiece lost + assert dummy.get_status_bit(bit=11) == 0 # no workpiece detected diff --git a/schunk_egu_egk_gripper_dummy/tests/test_requests.py b/schunk_egu_egk_gripper_dummy/tests/test_requests.py index 8677e72..c951791 100644 --- a/schunk_egu_egk_gripper_dummy/tests/test_requests.py +++ b/schunk_egu_egk_gripper_dummy/tests/test_requests.py @@ -118,8 +118,10 @@ def test_dummy_resets_success_status_bits_with_new_post_requests(): dummy = Dummy() dummy.set_status_bit(bit=13, value=True) # position reached dummy.set_status_bit(bit=4, value=True) # command successful + dummy.set_status_bit(bit=12, value=True) # workpiece gripped empty_command = "01" + "".zfill(30) # only fast stop active data = {"inst": dummy.plc_output, "value": empty_command} dummy.post(data) assert dummy.get_status_bit(bit=13) == 0 assert dummy.get_status_bit(bit=4) == 0 + assert dummy.get_status_bit(bit=12) == 0 diff --git a/schunk_egu_egk_gripper_tests/test/test_actions.py b/schunk_egu_egk_gripper_tests/test/test_actions.py index a4003c5..41fadf8 100644 --- a/schunk_egu_egk_gripper_tests/test/test_actions.py +++ b/schunk_egu_egk_gripper_tests/test/test_actions.py @@ -2,6 +2,7 @@ from test.conftest import launch_description from test.helpers import check_each_in, ActionReturnsResult, get_current_state from schunk_egu_egk_gripper_interfaces.action import ( # type: ignore[attr-defined] + GripWithVelocity, MoveToAbsolutePosition, MoveToRelativePosition, ReleaseWorkpiece, @@ -22,6 +23,21 @@ def test_driver_advertices_all_relevant_actions(running_driver): check_each_in(action_list, "get_topic_names_and_types") +@pytest.mark.launch(fixture=launch_description) +def test_driver_grips(running_driver): + goal = ( + GripWithVelocity.Goal() + ) # Our dummy is an EGK, which uses an additional velocity + goal.gripping_force = 65.0 # % of max force + goal.velocity_of_movement = 11.0 + goal.grip_direction = False # close + action = ActionReturnsResult("/grip", GripWithVelocity, goal) + action.event.wait() + assert action.result.workpiece_gripped + assert action.result.no_workpiece_detected is False + assert action.result.workpiece_lost is False + + @pytest.mark.launch(fixture=launch_description) def test_driver_moves_to_absolute_position(running_driver): test_positions = [43.55, 17.02, 38.55, 103.7] From 0d70a947f1f96379c06bff605b13630c3e55d6a0 Mon Sep 17 00:00:00 2001 From: Stefan Scherzinger Date: Fri, 23 Aug 2024 17:23:57 +0200 Subject: [PATCH 22/25] Add a test for the driver's `/grip_with_position` action Also drop setting the failure-related status bits 11 and 16. We'll implement them later in the dummy once we properly test them in our `test_actions.py`. --- schunk_egu_egk_gripper_dummy/src/dummy.py | 8 ++++++-- .../tests/test_dummy.py | 11 +++++++++-- .../test/test_actions.py | 19 +++++++++++++++++++ 3 files changed, 34 insertions(+), 4 deletions(-) diff --git a/schunk_egu_egk_gripper_dummy/src/dummy.py b/schunk_egu_egk_gripper_dummy/src/dummy.py index f766f51..3115813 100644 --- a/schunk_egu_egk_gripper_dummy/src/dummy.py +++ b/schunk_egu_egk_gripper_dummy/src/dummy.py @@ -358,5 +358,9 @@ def process_control_bits(self) -> None: if self.get_control_bit(bit=12) == 1: self.set_status_bit(bit=12, value=True) self.set_status_bit(bit=4, value=True) - self.set_status_bit(bit=11, value=False) - self.set_status_bit(bit=16, value=False) + + # Grip workpiece at position + if self.get_control_bit(bit=16) == 1: + self.set_status_bit(bit=12, value=True) + self.set_status_bit(bit=4, value=True) + self.set_status_bit(bit=31, value=True) diff --git a/schunk_egu_egk_gripper_dummy/tests/test_dummy.py b/schunk_egu_egk_gripper_dummy/tests/test_dummy.py index 1646d82..26f8c4f 100644 --- a/schunk_egu_egk_gripper_dummy/tests/test_dummy.py +++ b/schunk_egu_egk_gripper_dummy/tests/test_dummy.py @@ -199,5 +199,12 @@ def test_dummy_supports_grip(): dummy.process_control_bits() assert dummy.get_status_bit(bit=12) == 1 # workpiece gripped assert dummy.get_status_bit(bit=4) == 1 # command successfully processed - assert dummy.get_status_bit(bit=16) == 0 # workpiece lost - assert dummy.get_status_bit(bit=11) == 0 # no workpiece detected + + +def test_dummy_supports_grip_at_position(): + dummy = Dummy() + dummy.set_control_bit(bit=16, value=True) # grip workpiece at expected position + dummy.process_control_bits() + assert dummy.get_status_bit(bit=12) == 1 # workpiece gripped + assert dummy.get_status_bit(bit=4) == 1 # command successfully processed + assert dummy.get_status_bit(bit=31) == 1 # GPE activated diff --git a/schunk_egu_egk_gripper_tests/test/test_actions.py b/schunk_egu_egk_gripper_tests/test/test_actions.py index 41fadf8..29665d9 100644 --- a/schunk_egu_egk_gripper_tests/test/test_actions.py +++ b/schunk_egu_egk_gripper_tests/test/test_actions.py @@ -3,6 +3,7 @@ from test.helpers import check_each_in, ActionReturnsResult, get_current_state from schunk_egu_egk_gripper_interfaces.action import ( # type: ignore[attr-defined] GripWithVelocity, + GripWithPositionAndVelocity, MoveToAbsolutePosition, MoveToRelativePosition, ReleaseWorkpiece, @@ -38,6 +39,24 @@ def test_driver_grips(running_driver): assert action.result.workpiece_lost is False +@pytest.mark.launch(fixture=launch_description) +def test_driver_grips_with_position(running_driver): + goal = ( + GripWithPositionAndVelocity.Goal() + ) # Our dummy is an EGK, which uses an additional velocity + goal.absolute_position = 18.88 + goal.gripping_force = 65.0 # % of max force + goal.velocity_of_movement = 13.0 + goal.grip_direction = False # close + action = ActionReturnsResult( + "/grip_with_position", GripWithPositionAndVelocity, goal + ) + action.event.wait() + assert action.result.workpiece_gripped + assert action.result.no_workpiece_detected is False + assert action.result.workpiece_lost is False + + @pytest.mark.launch(fixture=launch_description) def test_driver_moves_to_absolute_position(running_driver): test_positions = [43.55, 17.02, 38.55, 103.7] From 867751ce53fa526412059da034e9495145ee3100 Mon Sep 17 00:00:00 2001 From: Stefan Scherzinger Date: Mon, 26 Aug 2024 09:23:58 +0200 Subject: [PATCH 23/25] Activate the CI pipeline for ROS2 `Jazzy` Also upgrade the `actions/checkout` version. --- .../workflows/industrial_ci_humble_action.yml | 2 +- .../workflows/industrial_ci_iron_action.yml | 2 +- .../workflows/industrial_ci_jazzy_action.yml | 24 +++++++++++++++++++ .../industrial_ci_rolling_action.yml | 2 +- README.md | 3 +++ 5 files changed, 30 insertions(+), 3 deletions(-) create mode 100644 .github/workflows/industrial_ci_jazzy_action.yml diff --git a/.github/workflows/industrial_ci_humble_action.yml b/.github/workflows/industrial_ci_humble_action.yml index 7c1c1d9..7e3308b 100644 --- a/.github/workflows/industrial_ci_humble_action.yml +++ b/.github/workflows/industrial_ci_humble_action.yml @@ -19,6 +19,6 @@ jobs: - {ROS_DISTRO: humble, ROS_REPO: main} runs-on: ubuntu-latest steps: - - uses: actions/checkout@v1 + - uses: actions/checkout@v4 - uses: 'ros-industrial/industrial_ci@master' env: ${{matrix.env}} diff --git a/.github/workflows/industrial_ci_iron_action.yml b/.github/workflows/industrial_ci_iron_action.yml index 4e518c0..3d1ffd4 100644 --- a/.github/workflows/industrial_ci_iron_action.yml +++ b/.github/workflows/industrial_ci_iron_action.yml @@ -19,6 +19,6 @@ jobs: - {ROS_DISTRO: iron, ROS_REPO: main} runs-on: ubuntu-latest steps: - - uses: actions/checkout@v1 + - uses: actions/checkout@v4 - uses: 'ros-industrial/industrial_ci@master' env: ${{matrix.env}} diff --git a/.github/workflows/industrial_ci_jazzy_action.yml b/.github/workflows/industrial_ci_jazzy_action.yml new file mode 100644 index 0000000..ec906f8 --- /dev/null +++ b/.github/workflows/industrial_ci_jazzy_action.yml @@ -0,0 +1,24 @@ +name: Jazzy + +on: + push: + pull_request: + schedule: + # Run every Friday at 6:30 am to detect breaking APIs + - cron: '30 6 * * 5' + + +jobs: + industrial_ci: + env: + BEFORE_BUILD_TARGET_WORKSPACE: '.github/script/install_dependencies.sh' + strategy: + fail-fast: false + matrix: + env: + - {ROS_DISTRO: jazzy, ROS_REPO: main} + runs-on: ubuntu-latest + steps: + - uses: actions/checkout@v4 + - uses: 'ros-industrial/industrial_ci@master' + env: ${{matrix.env}} diff --git a/.github/workflows/industrial_ci_rolling_action.yml b/.github/workflows/industrial_ci_rolling_action.yml index 78a21f9..6868f50 100644 --- a/.github/workflows/industrial_ci_rolling_action.yml +++ b/.github/workflows/industrial_ci_rolling_action.yml @@ -20,6 +20,6 @@ jobs: - {ROS_DISTRO: rolling, ROS_REPO: main} runs-on: ubuntu-latest steps: - - uses: actions/checkout@v1 + - uses: actions/checkout@v4 - uses: 'ros-industrial/industrial_ci@master' env: ${{matrix.env}} diff --git a/README.md b/README.md index 930184a..1c25ce4 100644 --- a/README.md +++ b/README.md @@ -14,6 +14,9 @@ build badge iron + + build badge jazzy + build badge rolling From 2355856e8b77d809659b402d86031f0677b00ccb Mon Sep 17 00:00:00 2001 From: Stefan Scherzinger Date: Mon, 26 Aug 2024 13:09:36 +0200 Subject: [PATCH 24/25] Lower expected accuracy for dummy tests with conversions MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit This makes them more robust against these unnecessary CI failures: `assert 76.9995346069336 == 77.0 ± 7.7e-05` --- schunk_egu_egk_gripper_dummy/tests/test_dummy.py | 6 ++++-- .../tests/test_plc_communication.py | 4 ++-- 2 files changed, 6 insertions(+), 4 deletions(-) diff --git a/schunk_egu_egk_gripper_dummy/tests/test_dummy.py b/schunk_egu_egk_gripper_dummy/tests/test_dummy.py index 26f8c4f..557fb4b 100644 --- a/schunk_egu_egk_gripper_dummy/tests/test_dummy.py +++ b/schunk_egu_egk_gripper_dummy/tests/test_dummy.py @@ -76,7 +76,9 @@ def test_dummy_moves_to_absolute_position(): } dummy.post(command) - assert dummy.get_actual_position() == pytest.approx(target_pos / 1000.0) + assert dummy.get_actual_position() == pytest.approx( + target_pos / 1000.0, rel=1e-3 + ) assert dummy.get_status_bit(bit=13) == 1 # position reached assert dummy.get_status_bit(bit=4) == 1 # command successfully processed @@ -97,7 +99,7 @@ def test_dummy_moves_to_relative_position(): dummy.post(command) after = dummy.get_actual_position() assert after < before # we are decreasing - assert after == pytest.approx(before + target_pos / 1000.0) + assert after == pytest.approx(before + target_pos / 1000.0, rel=1e-3) assert dummy.get_status_bit(bit=13) == 1 # position reached assert dummy.get_status_bit(bit=4) == 1 # command successfully processed diff --git a/schunk_egu_egk_gripper_dummy/tests/test_plc_communication.py b/schunk_egu_egk_gripper_dummy/tests/test_plc_communication.py index 574e9b4..cee1a03 100644 --- a/schunk_egu_egk_gripper_dummy/tests/test_plc_communication.py +++ b/schunk_egu_egk_gripper_dummy/tests/test_plc_communication.py @@ -125,14 +125,14 @@ def test_dummy_supports_reading_target_position(): dummy = Dummy() target_pos = 12300 # um dummy.plc_output_buffer[4:8] = bytes(struct.pack("i", target_pos)) - assert pytest.approx(dummy.get_target_position()) == target_pos / 1000.0 + assert pytest.approx(dummy.get_target_position(), rel=1e-3) == target_pos / 1000.0 def test_dummy_supports_reading_target_speed(): dummy = Dummy() target_speed = 55300 dummy.plc_output_buffer[8:12] = bytes(struct.pack("i", target_speed)) - assert pytest.approx(dummy.get_target_speed()) == target_speed / 1000.0 + assert pytest.approx(dummy.get_target_speed(), rel=1e-3) == target_speed / 1000.0 def test_dummy_supports_writing_actual_position(): From 75f42ffb5bd17bf2136f7ed25d7c517890558116 Mon Sep 17 00:00:00 2001 From: Stefan Scherzinger Date: Mon, 26 Aug 2024 17:14:47 +0200 Subject: [PATCH 25/25] Add a test for the gripper's `gripper_control` action Also add meaningful values for `max_grip_force`, `min_grip_force`, `max_vel`, and `min_vel` (EGK 40). --- schunk_egu_egk_gripper_dummy/config/data.json | 8 ++++---- .../test/test_actions.py | 20 +++++++++++++++++++ 2 files changed, 24 insertions(+), 4 deletions(-) diff --git a/schunk_egu_egk_gripper_dummy/config/data.json b/schunk_egu_egk_gripper_dummy/config/data.json index 470c567..b28eda0 100644 --- a/schunk_egu_egk_gripper_dummy/config/data.json +++ b/schunk_egu_egk_gripper_dummy/config/data.json @@ -72,19 +72,19 @@ "00000000" ], "0x0628": [ - "40B00000" + "0000B040" ], "0x0630": [ - "42E60000" + "0000E642" ], "0x0650": [ "41B00000" ], "0x0658": [ - "42960000" + "00009642" ], "0x0660": [ - "43160000" + "0000B442" ], "0x06A8": [ "00000000" diff --git a/schunk_egu_egk_gripper_tests/test/test_actions.py b/schunk_egu_egk_gripper_tests/test/test_actions.py index 29665d9..6524906 100644 --- a/schunk_egu_egk_gripper_tests/test/test_actions.py +++ b/schunk_egu_egk_gripper_tests/test/test_actions.py @@ -8,6 +8,7 @@ MoveToRelativePosition, ReleaseWorkpiece, ) +from control_msgs.action import GripperCommand @pytest.mark.launch(fixture=launch_description) @@ -57,6 +58,25 @@ def test_driver_grips_with_position(running_driver): assert action.result.workpiece_lost is False +@pytest.mark.launch(fixture=launch_description) +def test_driver_supports_gripper_control(running_driver): + + runs = [ + {"position": 0.0, "max_effort": 77.0}, # grip workpiece + {"position": 82.4, "max_effort": 0.0}, # move to abs. position + {"position": 66.3, "max_effort": 85.1}, # grip with position + ] + + for run in runs: + goal = GripperCommand.Goal() + goal.command.position = run["position"] + goal.command.max_effort = run["max_effort"] + action = ActionReturnsResult("/gripper_control", GripperCommand, goal) + action.event.wait() + assert action.result.reached_goal + assert action.result.stalled + + @pytest.mark.launch(fixture=launch_description) def test_driver_moves_to_absolute_position(running_driver): test_positions = [43.55, 17.02, 38.55, 103.7]