Report operational status with mock hardware (#1923)

Author: Daniiiil1

ur_robot_driver/test/test_mock_hardware.py

@@ -37,8 +37,11 @@
 import rclpy
 from geometry_msgs.msg import Inertia, Vector3
 from rclpy.node import Node
+from rclpy.qos import DurabilityPolicy, QoSProfile, ReliabilityPolicy
 from control_msgs.action import FollowJointTrajectory
 from controller_manager_msgs.srv import SwitchController
+from std_msgs.msg import Bool
+from ur_dashboard_msgs.msg import RobotMode, SafetyMode
 
 sys.path.append(os.path.dirname(__file__))
 from test_common import (  # noqa: E402
@@ -100,6 +103,46 @@ def test_get_robot_software_version(self):
             self._configuration_controller_interface.get_robot_software_version().major, 1
         )
 
+    def test_mock_hardware_publishes_operational_status(self):
+        """Mock hardware reports deterministic happy-path status on latched topics."""
+        messages = {}
+        qos = QoSProfile(
+            depth=1,
+            durability=DurabilityPolicy.TRANSIENT_LOCAL,
+            reliability=ReliabilityPolicy.RELIABLE,
+        )
+        subscriptions = [
+            self.node.create_subscription(
+                RobotMode,
+                "/io_and_status_controller/robot_mode",
+                lambda msg: messages.setdefault("robot_mode", msg.mode),
+                qos,
+            ),
+            self.node.create_subscription(
+                SafetyMode,
+                "/io_and_status_controller/safety_mode",
+                lambda msg: messages.setdefault("safety_mode", msg.mode),
+                qos,
+            ),
+            self.node.create_subscription(
+                Bool,
+                "/io_and_status_controller/robot_program_running",
+                lambda msg: messages.setdefault("program_running", msg.data),
+                qos,
+            ),
+        ]
+
+        deadline = time.monotonic() + 10.0
+        while len(messages) < 3 and time.monotonic() < deadline:
+            rclpy.spin_once(self.node, timeout_sec=0.1)
+
+        self.assertEqual(messages.get("robot_mode"), RobotMode.RUNNING)
+        self.assertEqual(messages.get("safety_mode"), SafetyMode.NORMAL)
+        self.assertIs(messages.get("program_running"), True)
+
+        for subscription in subscriptions:
+            self.node.destroy_subscription(subscription)
+
     def test_start_jtc_controller(self):
         # Deactivate controller, if it is not already
         self.assertTrue(

ur_robot_driver/urdf/ur.ros2_control.xacro

@@ -186,13 +186,21 @@
         <state_interface name="tool_analog_input_type_0"/>
         <state_interface name="tool_analog_input_type_1"/>
 
-        <state_interface name="robot_mode"/>
+        <state_interface name="robot_mode">
+          <xacro:if value="${use_mock_hardware}">
+            <param name="initial_value">7</param>
+          </xacro:if>
+        </state_interface>
         <state_interface name="robot_status_bit_0"/>
         <state_interface name="robot_status_bit_1"/>
         <state_interface name="robot_status_bit_2"/>
         <state_interface name="robot_status_bit_3"/>
 
-        <state_interface name="safety_mode"/>
+        <state_interface name="safety_mode">
+          <xacro:if value="${use_mock_hardware}">
+            <param name="initial_value">1</param>
+          </xacro:if>
+        </state_interface>
         <state_interface name="safety_status_bit_0"/>
         <state_interface name="safety_status_bit_1"/>
         <state_interface name="safety_status_bit_2"/>
@@ -205,7 +213,11 @@
         <state_interface name="safety_status_bit_9"/>
         <state_interface name="safety_status_bit_10"/>
 
-        <state_interface name="program_running"/>
+        <state_interface name="program_running">
+          <xacro:if value="${use_mock_hardware}">
+            <param name="initial_value">1</param>
+          </xacro:if>
+        </state_interface>
       </gpio>
 
       <gpio name="${tf_prefix}payload">