fix(arcor2_ur): checking for robot's state

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

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:

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"])

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"])

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

src/python/arcor2_ur/VERSION

@@ -1 +1 @@
-1.4.0
+1.4.1

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"],
+)

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()

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()

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")

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)

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"