From 2b2f7bdccf73b7af276cb02b3b612142ba6da964 Mon Sep 17 00:00:00 2001 From: ZdenekM Date: Thu, 21 Nov 2024 13:20:55 +0100 Subject: [PATCH] fix(arcor2_ur): checking for robot's state --- compose-files/ur-demo/docker-compose.sim.yml | 2 +- compose-files/ur-demo/docker-compose.yml | 4 +- src/docker/arcor2_ur/BUILD | 2 +- src/docker/arcor2_ur_ot/BUILD | 2 +- src/python/arcor2_ur/CHANGELOG.md | 6 ++ src/python/arcor2_ur/VERSION | 2 +- src/python/arcor2_ur/scripts/BUILD | 6 ++ .../arcor2_ur/scripts/robot_publisher.py | 34 +++++++ src/python/arcor2_ur/scripts/ur.py | 99 ++++++++++--------- src/python/arcor2_ur/tests/BUILD | 4 +- src/python/arcor2_ur/tests/conftest.py | 11 ++- src/python/arcor2_ur/topics.py | 11 +++ 12 files changed, 127 insertions(+), 56 deletions(-) create mode 100644 src/python/arcor2_ur/scripts/robot_publisher.py create mode 100644 src/python/arcor2_ur/topics.py diff --git a/compose-files/ur-demo/docker-compose.sim.yml b/compose-files/ur-demo/docker-compose.sim.yml index ea158ef4..8f50e67b 100644 --- a/compose-files/ur-demo/docker-compose.sim.yml +++ b/compose-files/ur-demo/docker-compose.sim.yml @@ -1,6 +1,6 @@ services: ur-demo-ursim: - image: universalrobots/ursim_e-series:5.17.2 + image: universalrobots/ursim_e-series:5.17.3 # version should match version of the Polyscope on the robot container_name: ur-demo-ursim ports: # network_mode: host can't be (easily) used as ursim starts its own X server - "5900:5900" # VNC port diff --git a/compose-files/ur-demo/docker-compose.yml b/compose-files/ur-demo/docker-compose.yml index b317279d..e089b350 100644 --- a/compose-files/ur-demo/docker-compose.yml +++ b/compose-files/ur-demo/docker-compose.yml @@ -1,6 +1,6 @@ services: ur-demo-robot-api: - image: arcor2/arcor2_ur:1.4.0 + image: arcor2/arcor2_ur:1.4.1 container_name: ur-demo-robot-api ports: - "5012:5012" @@ -146,7 +146,7 @@ services: - ur-demo-asset ur-demo-upload-object-types: - image: arcor2/arcor2_ur_ot:1.4.0 + image: arcor2/arcor2_ur_ot:1.4.1 container_name: "ur-demo-upload-object-types" depends_on: ur-demo-project: diff --git a/src/docker/arcor2_ur/BUILD b/src/docker/arcor2_ur/BUILD index 3d03850a..f2fa5c56 100644 --- a/src/docker/arcor2_ur/BUILD +++ b/src/docker/arcor2_ur/BUILD @@ -1,2 +1,2 @@ shell_source(name="start.sh", source="start.sh") -docker_image(name="arcor2_ur", repository="arcor2/arcor2_ur", dependencies=[":start.sh", "build-support:install_ur_dependencies.sh"], image_tags=["1.4.0"]) +docker_image(name="arcor2_ur", repository="arcor2/arcor2_ur", dependencies=[":start.sh", "build-support:install_ur_dependencies.sh"], image_tags=["1.4.1"]) diff --git a/src/docker/arcor2_ur_ot/BUILD b/src/docker/arcor2_ur_ot/BUILD index e4a3f36a..b4b8718d 100644 --- a/src/docker/arcor2_ur_ot/BUILD +++ b/src/docker/arcor2_ur_ot/BUILD @@ -1 +1 @@ -docker_image(name="arcor2_ur_ot", repository="arcor2/arcor2_ur_ot", image_tags=["1.4.0"]) +docker_image(name="arcor2_ur_ot", repository="arcor2/arcor2_ur_ot", image_tags=["1.4.1"]) diff --git a/src/python/arcor2_ur/CHANGELOG.md b/src/python/arcor2_ur/CHANGELOG.md index 355165b3..aab79433 100644 --- a/src/python/arcor2_ur/CHANGELOG.md +++ b/src/python/arcor2_ur/CHANGELOG.md @@ -2,6 +2,12 @@ The format is based on [Keep a Changelog](https://keepachangelog.com/en/1.0.0/), +## [1.4.1] - 2024-11-21 + +### Fixed + +- Checking for robot's state - it should no longer be necessary to start the robot manually (before calling `PUT /state/start`). It should also work if the robot is already started. + ## [1.4.0] - 2024-11-06 ### Changed diff --git a/src/python/arcor2_ur/VERSION b/src/python/arcor2_ur/VERSION index e21e727f..13175fdc 100644 --- a/src/python/arcor2_ur/VERSION +++ b/src/python/arcor2_ur/VERSION @@ -1 +1 @@ -1.4.0 \ No newline at end of file +1.4.1 \ No newline at end of file diff --git a/src/python/arcor2_ur/scripts/BUILD b/src/python/arcor2_ur/scripts/BUILD index 33f95f20..c94d5e25 100644 --- a/src/python/arcor2_ur/scripts/BUILD +++ b/src/python/arcor2_ur/scripts/BUILD @@ -8,3 +8,9 @@ arcor2_pex_binary( arcor2_pex_binary( name="upload_objects", dependencies=["src/python/arcor2_ur/data/urdf:ur5e"], interpreter_constraints=["==3.12.*"] ) + +arcor2_pex_binary( + name="robot_publisher", + interpreter_constraints=["==3.12.*"], + dependencies=["3rdparty#PyYAML", "3rdparty#numpy"], +) \ No newline at end of file diff --git a/src/python/arcor2_ur/scripts/robot_publisher.py b/src/python/arcor2_ur/scripts/robot_publisher.py new file mode 100644 index 00000000..223a779c --- /dev/null +++ b/src/python/arcor2_ur/scripts/robot_publisher.py @@ -0,0 +1,34 @@ +import rclpy # pants: no-infer-dep +from rclpy.node import Node # pants: no-infer-dep +from rclpy.qos import QoSDurabilityPolicy, QoSProfile, QoSReliabilityPolicy # pants: no-infer-dep +from std_msgs.msg import Bool # pants: no-infer-dep +from ur_dashboard_msgs.msg import RobotMode # pants: no-infer-dep + +from arcor2_ur.topics import ROBOT_MODE_TOPIC, ROBOT_PROGRAM_RUNNING_TOPIC + + +class PublisherNode(Node): + def __init__(self) -> None: + super().__init__("minimal_publisher") + + qos = QoSProfile( + depth=1, durability=QoSDurabilityPolicy.TRANSIENT_LOCAL, reliability=QoSReliabilityPolicy.RELIABLE + ) + + self.robot_program_running_pub = self.create_publisher(Bool, ROBOT_PROGRAM_RUNNING_TOPIC, qos) + self.robot_mode_pub = self.create_publisher(RobotMode, ROBOT_MODE_TOPIC, qos) + + +def main() -> None: + rclpy.init() + publisher_node = PublisherNode() + publisher_node.robot_program_running_pub.publish(Bool(data=True)) + publisher_node.robot_mode_pub.publish(RobotMode(mode=RobotMode.RUNNING)) + rclpy.spin(publisher_node) + + publisher_node.destroy_node() + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/src/python/arcor2_ur/scripts/ur.py b/src/python/arcor2_ur/scripts/ur.py index 11cf98a5..36b3b432 100644 --- a/src/python/arcor2_ur/scripts/ur.py +++ b/src/python/arcor2_ur/scripts/ur.py @@ -7,7 +7,6 @@ import time from dataclasses import dataclass, field from functools import wraps -from threading import Event import rclpy # pants: no-infer-dep from ament_index_python.packages import get_package_share_directory # pants: no-infer-dep @@ -16,7 +15,9 @@ from geometry_msgs.msg import PoseStamped # pants: no-infer-dep from moveit.planning import MoveItPy, PlanningComponent # pants: no-infer-dep from moveit_configs_utils import MoveItConfigsBuilder # pants: no-infer-dep +from rclpy.callback_groups import MutuallyExclusiveCallbackGroup # pants: no-infer-dep from rclpy.node import Node # pants: no-infer-dep +from rclpy.qos import QoSDurabilityPolicy, QoSProfile, QoSReliabilityPolicy # pants: no-infer-dep from sensor_msgs.msg import JointState # pants: no-infer-dep from std_msgs.msg import Bool, String # pants: no-infer-dep from std_srvs.srv import Trigger # pants: no-infer-dep @@ -34,7 +35,7 @@ from arcor2.flask import RespT, create_app, run_app from arcor2.helpers import port_from_url from arcor2.logging import get_logger -from arcor2_ur import get_data, version +from arcor2_ur import get_data, topics, version from arcor2_ur.exceptions import StartError, UrGeneral, WebApiError from arcor2_ur.object_types.ur5e import Vacuum from arcor2_ur.vgc10 import VGC10 @@ -48,21 +49,9 @@ PLANNING_GROUP_NAME = os.getenv("ARCOR2_UR_PLANNING_GROUP_NAME", "ur_manipulator") ROBOT_IP = os.getenv("ARCOR2_UR_ROBOT_IP", "") VGC10_PORT = env.get_int("ARCOR2_UR_VGC10_PORT", 54321) - -SERVICE_NAME = f"UR Web API ({UR_TYPE})" - INTERACT_WITH_DASHBOARD = env.get_bool("ARCOR2_UR_INTERACT_WITH_DASHBOARD", True) -DASHBOARD_CLIENT_NS = "/dashboard_client" -BRAKE_RELEASE_SRV = f"{DASHBOARD_CLIENT_NS}/brake_release" -POWER_OFF_SRV = f"{DASHBOARD_CLIENT_NS}/power_off" -LOAD_PROGRAM_SRV = f"{DASHBOARD_CLIENT_NS}/load_program" -PLAY_SRV = f"{DASHBOARD_CLIENT_NS}/play" -IO_AND_STATUS_CONTROLLER_NS = "/io_and_status_controller" -ROBOT_MODE_TOPIC = f"{IO_AND_STATUS_CONTROLLER_NS}/robot_mode" -ROBOT_PROGRAM_RUNNING_TOPIC = f"{IO_AND_STATUS_CONTROLLER_NS}/robot_program_running" -SET_SPEED_SLIDER_SRV = f"{IO_AND_STATUS_CONTROLLER_NS}/set_speed_slider" -SET_PAYLOAD_SRV = f"{IO_AND_STATUS_CONTROLLER_NS}/set_payload" +SERVICE_NAME = f"UR Web API ({UR_TYPE})" def plan_and_execute( @@ -102,66 +91,83 @@ def __init__(self, interact_with_dashboard=True) -> None: # TODO makes no difference for moveitpy :-/ rclpy.logging.set_logger_level("ur_api_node", rclpy.logging.LoggingSeverity.DEBUG) + sub_qos = QoSProfile( + depth=1, durability=QoSDurabilityPolicy.TRANSIENT_LOCAL, reliability=QoSReliabilityPolicy.RELIABLE + ) + self.buffer = Buffer() self.listener = TransformListener(self.buffer, self) - self.subscription = self.create_subscription(JointState, "joint_states", self.joint_states_cb, 10) + self.subscription = self.create_subscription( + JointState, "joint_states", self.joint_states_cb, 10, callback_group=MutuallyExclusiveCallbackGroup() + ) self.interact_with_dashboard = interact_with_dashboard self.robot_mode: RobotMode | None = None - self.robot_mode_evt = Event() - self.robot_program_running_evt = Event() + self.robot_program_running: bool | None = None - self._break_release_client = self.create_client(Trigger, BRAKE_RELEASE_SRV) - self._power_off_client = self.create_client(Trigger, POWER_OFF_SRV) - self._load_program_client = self.create_client(Load, LOAD_PROGRAM_SRV) - self._play_client = self.create_client(Trigger, PLAY_SRV) - self._set_speed_slider_client = self.create_client(SetSpeedSliderFraction, SET_SPEED_SLIDER_SRV) - self._set_payload_client = self.create_client(SetPayload, SET_PAYLOAD_SRV) + self._break_release_client = self.create_client(Trigger, topics.BRAKE_RELEASE_SRV) + self._power_off_client = self.create_client(Trigger, topics.POWER_OFF_SRV) + self._load_program_client = self.create_client(Load, topics.LOAD_PROGRAM_SRV) + self._play_client = self.create_client(Trigger, topics.PLAY_SRV) + self._set_speed_slider_client = self.create_client(SetSpeedSliderFraction, topics.SET_SPEED_SLIDER_SRV) + self._set_payload_client = self.create_client(SetPayload, topics.SET_PAYLOAD_SRV) self.script_cmd_pub = self.create_publisher(String, "/urscript_interface/script_command", 10) - self.robot_mode_sub = self.create_subscription(RobotMode, ROBOT_MODE_TOPIC, self.robot_mode_cb, 10) + self.robot_mode_sub = self.create_subscription( + RobotMode, + topics.ROBOT_MODE_TOPIC, + self.robot_mode_cb, + sub_qos, + callback_group=MutuallyExclusiveCallbackGroup(), + ) self.robot_program_running_sub = self.create_subscription( - Bool, ROBOT_PROGRAM_RUNNING_TOPIC, self.robot_program_running_cb, 10 + Bool, + topics.ROBOT_PROGRAM_RUNNING_TOPIC, + self.robot_program_running_cb, + sub_qos, + callback_group=MutuallyExclusiveCallbackGroup(), ) def wait_for_services(self) -> None: while self.interact_with_dashboard and not self._break_release_client.wait_for_service(timeout_sec=1.0): - logger.warning(f"Service {BRAKE_RELEASE_SRV} not available, waiting again...") + logger.warning(f"Service {topics.BRAKE_RELEASE_SRV} not available, waiting again...") while self.interact_with_dashboard and not self._power_off_client.wait_for_service(timeout_sec=1.0): - logger.warning(f"Service {POWER_OFF_SRV} not available, waiting again...") + logger.warning(f"Service {topics.POWER_OFF_SRV} not available, waiting again...") while self.interact_with_dashboard and not self._load_program_client.wait_for_service(timeout_sec=1.0): - logger.warning(f"Service {LOAD_PROGRAM_SRV} not available, waiting again...") + logger.warning(f"Service {topics.LOAD_PROGRAM_SRV} not available, waiting again...") while self.interact_with_dashboard and not self._play_client.wait_for_service(timeout_sec=1.0): - logger.warning(f"Service {PLAY_SRV} not available, waiting again...") + logger.warning(f"Service {topics.PLAY_SRV} not available, waiting again...") while not self._set_speed_slider_client.wait_for_service(timeout_sec=1.0): - logger.warning(f"Service {SET_SPEED_SLIDER_SRV} not available, waiting again...") + logger.warning(f"Service {topics.SET_SPEED_SLIDER_SRV} not available, waiting again...") while not self._set_payload_client.wait_for_service(timeout_sec=1.0): - logger.warning(f"Service {SET_PAYLOAD_SRV} not available, waiting again...") + logger.warning(f"Service {topics.SET_PAYLOAD_SRV} not available, waiting again...") def robot_program_running_cb(self, msg: Bool) -> None: logger.info(f"Program running: {msg.data}") - if msg.data: - self.robot_program_running_evt.set() - else: - self.robot_program_running_evt.clear() + self.robot_program_running = msg.data def robot_mode_cb(self, msg: RobotMode) -> None: logger.info(f"Robot mode: {msg.mode}") self.robot_mode = msg - self.robot_mode_evt.set() - def wait_for_robot_mode(self, mode: RobotMode, timeout=30) -> None: - while self.robot_mode is None or self.robot_mode.mode != mode: - if not self.robot_mode_evt.wait(timeout): - if self.robot_mode is None: - raise TimeoutError("Didn't get any robot mode.") - raise TimeoutError(f"Current mode {self.robot_mode.mode}, was waiting for {mode}.") - self.robot_mode_evt.clear() + def wait_for_robot_mode(self, mode: set[RobotMode], timeout=30) -> None: + start = time.monotonic() + while self.robot_mode is None or self.robot_mode.mode not in mode: + time.sleep(0.1) + if time.monotonic() - start > timeout: + raise TimeoutError(f"Timeout when waiting for RobotMode={mode}.") + + def wait_for_program_running(self, timeout=10) -> None: + start = time.monotonic() + while not self.robot_program_running: + time.sleep(0.1) + if time.monotonic() - start > timeout: + raise TimeoutError("Timeout when waiting for program running.") def urscript(self, src: str) -> None: msg = String() @@ -374,11 +380,10 @@ def put_start() -> RespT: try: node.brake_release() - # node.wait_for_robot_mode(RobotMode.RUNNING) # TODO why wtf is RobotMode not received?? + node.wait_for_robot_mode({RobotMode.RUNNING, RobotMode.IDLE}) node.load_program() node.play() - # if not node.robot_program_running_evt.wait(10): - # raise TimeoutError("Robot program not running.") + node.wait_for_program_running() except Exception: node.destroy_node() diff --git a/src/python/arcor2_ur/tests/BUILD b/src/python/arcor2_ur/tests/BUILD index 15a363f7..a130fb6e 100644 --- a/src/python/arcor2_ur/tests/BUILD +++ b/src/python/arcor2_ur/tests/BUILD @@ -1,8 +1,8 @@ python_tests( runtime_package_dependencies=[ - "src/python/arcor2_ur/scripts:ur", + "src/python/arcor2_ur/scripts:ur", "src/python/arcor2_ur/scripts:robot_publisher", ], interpreter_constraints=["==3.12.*"], ) -python_test_utils(name="test_utils") +python_test_utils(name="test_utils") \ No newline at end of file diff --git a/src/python/arcor2_ur/tests/conftest.py b/src/python/arcor2_ur/tests/conftest.py index 8f4cd017..4f810e4c 100644 --- a/src/python/arcor2_ur/tests/conftest.py +++ b/src/python/arcor2_ur/tests/conftest.py @@ -71,11 +71,20 @@ def start_processes(request) -> Iterator[Urls]: raise Exception("Robot service died.") try: - check_health("UR", robot_url, timeout=10) + check_health("UR", robot_url, timeout=20) except CheckHealthException: finish_processes(processes) raise + # robot_mode etc. is not published with mock_hw -> there is this helper node to do that + # it can't be published from here as it depends on ROS (Python 3.12) + robot_pub_proc = sp.Popen(["python", "src.python.arcor2_ur.scripts/robot_publisher.pex"], **kwargs) # type: ignore + processes.append(robot_pub_proc) + + if robot_pub_proc.poll(): + finish_processes(processes) + raise Exception("Robot publisher node died.") + yield Urls(ros_domain_id, robot_url) finish_processes(processes) diff --git a/src/python/arcor2_ur/topics.py b/src/python/arcor2_ur/topics.py new file mode 100644 index 00000000..4e16fea3 --- /dev/null +++ b/src/python/arcor2_ur/topics.py @@ -0,0 +1,11 @@ +DASHBOARD_CLIENT_NS = "/dashboard_client" +BRAKE_RELEASE_SRV = f"{DASHBOARD_CLIENT_NS}/brake_release" +POWER_OFF_SRV = f"{DASHBOARD_CLIENT_NS}/power_off" +LOAD_PROGRAM_SRV = f"{DASHBOARD_CLIENT_NS}/load_program" +PLAY_SRV = f"{DASHBOARD_CLIENT_NS}/play" + +IO_AND_STATUS_CONTROLLER_NS = "/io_and_status_controller" +ROBOT_MODE_TOPIC = f"{IO_AND_STATUS_CONTROLLER_NS}/robot_mode" +ROBOT_PROGRAM_RUNNING_TOPIC = f"{IO_AND_STATUS_CONTROLLER_NS}/robot_program_running" +SET_SPEED_SLIDER_SRV = f"{IO_AND_STATUS_CONTROLLER_NS}/set_speed_slider" +SET_PAYLOAD_SRV = f"{IO_AND_STATUS_CONTROLLER_NS}/set_payload"