Update workstation UI, ROS task control and device integration

station/test/ros_fixture.py

@@ -64,6 +64,7 @@ def main():
         node.sync_streams();assert node._streams.items=={}
         camera.watch({'left_color'},set());node.sync_streams()
         assert set(node._streams.items)=={'left_color'}
+        assert all(node._accept('left_color') for _ in range(10)), 'color preview must preserve input cadence'
         from sensor_msgs.msg import Image
         from rclpy.qos import qos_profile_sensor_data
         publisher=node.create_publisher(Image,'/left/color/image_raw',qos_profile_sensor_data)
@@ -95,6 +96,26 @@ def main():
         node.destroy_node();rclpy.shutdown()
 
 
+    # Real-camera UI preview uses a compressed transport; perception stays raw.
+    import cv2,numpy as np
+    from sensor_msgs.msg import CompressedImage
+    camera.watch({'compressed_test_color'},set())
+    node=camera._open_node([{'id':'compressed_test','role':'wrist','preview_transport':'compressed'}])
+    try:
+        node.sync_streams()
+        publisher=node.create_publisher(CompressedImage,'/compressed_test/color/preview/compressed',qos_profile_sensor_data)
+        ok,jpg=cv2.imencode('.jpg',np.full((720,1280,3),(20,80,150),np.uint8));assert ok
+        sample=CompressedImage();sample.format='jpeg';sample.data=jpg.tobytes()
+        deadline=time.monotonic()+3
+        while 'compressed_test_color' not in camera.previews() and time.monotonic()<deadline:
+            publisher.publish(sample);rclpy.spin_once(node,timeout_sec=.05);camera.publish_latest()
+        blob,w,h=camera.previews()['compressed_test_color']
+        assert (w,h)==(1280,720),(w,h)
+        assert len(blob)==w*h*3
+        print('ROS compressed preview preserves camera resolution PASS',flush=True)
+    finally:
+        camera.watch(set(),set());node.destroy_node();rclpy.shutdown()
+
     # Exercise native Action feedback/cancellation without a robot controller.
     from station.ros_gateway import RosGateway
     from arm_application.site_io import load_site_dict
@@ -133,6 +154,31 @@ def main():
     arm=next(a for a in gateway.arms if a['id']==gateway.active_arm)
     msg=JointState();msg.name=[f"{arm['sn']}_joint{i}" for i in range(1,8)];msg.position=[0.]*7
     try:
+        from unittest.mock import patch
+        from flexiv_msgs.msg import RobotStates
+        from station.operator_arm_state import tcp_display_pose
+        import math
+        assert tcp_display_pose((0.,0.,0.),(0.,0.,0.,0.)) is None
+        yaw_pose=tcp_display_pose((.1,.2,.3),(0.,0.,math.sqrt(.5),math.sqrt(.5)))
+        assert yaw_pose[:3]==[100.,200.,300.] and abs(yaw_pose[-1]-90.)<1e-8
+        native=RobotStates()
+        native.tcp_pose.pose.position.x=.5
+        native.tcp_pose.pose.orientation.w=1.
+        aid=arm['id']
+        with patch('station.ros_gateway.time.monotonic',return_value=100.):
+            gateway._joint(msg);native.robot_timestamp.sec=10;gateway._native_state(aid,native)
+            assert gateway.states()[0][aid]['enabled'] is None
+        with patch('station.ros_gateway.time.monotonic',return_value=100.1):
+            gateway._joint(msg);native.robot_timestamp.sec=11;gateway._native_state(aid,native)
+            assert gateway.states()[0][aid]['enabled'] is True
+            assert gateway.states()[0][aid]['tcp_pose']==[500.,0.,0.,0.,0.,0.]
+        with patch('station.ros_gateway.time.monotonic',return_value=102.):
+            gateway._joint(msg);gateway._native_state(aid,native)
+            assert gateway.states()[0][aid]['pose_live'] is True
+            assert gateway.states()[0][aid]['tcp_pose'] is None
+            assert gateway.states()[0][aid]['enabled'] is None, 'repeated cached hardware sample must not confirm enable'
+        gateway._robot_clock.clear()
+        gateway._last_joint_sample=0.
         gateway._joint(msg)
         assert gateway.move_joints(arm,[.01]*7,.15,threading.Event())['ok']
         gateway._joint(msg)
@@ -152,7 +198,12 @@ def main():
             assert gateway.confirm_stopped(arm)['ok']
             assert controller_state[0]=='inactive'
             assert switches[-1]==([],['rizon_arm_controller'])
-            # Only the next explicit command restores the controller.
+            # Enable/readiness checks must not resume a stopped trajectory.
+            switch_count = len(switches)
+            gateway.require_arm(arm['id'],threading.Event(),resume=False)
+            assert controller_state[0]=='inactive'
+            assert len(switches)==switch_count
+            # Only the next explicit motion command restores the controller.
             gateway.require_arm(arm['id'],threading.Event())
             assert controller_state[0]=='active'
             assert switches[-1]==(['rizon_arm_controller'],[])
@@ -164,12 +215,45 @@ def main():
         finally:
             joint_stop.set();joint_thread.join()
         print('ROS stop deactivation, stationary confirmation, explicit resume and arm ownership PASS',flush=True)
+        gateway._motion_watch.add(arm['id'])
+        gateway._robot_clock[arm['id']] = (123, time.monotonic())
+        assert gateway.motion_interruption(arm['id']) is None
+        gateway._robot_clock[arm['id']] = (123, time.monotonic()-1.)
+        assert gateway.motion_interruption(arm['id'])
+        assert gateway.motion_interruption(arm['id']) is None
+        # A recovered Enable sample does not recreate the cancelled motion watch/task.
+        gateway._robot_clock[arm['id']] = (124, time.monotonic())
+        assert arm['id'] not in gateway._motion_watch
+        gateway._robot_clock.clear()
+        print('ROS operational feedback loss cancels old motion once; no automatic resume PASS',flush=True)
+        from std_msgs.msg import Float32MultiArray
+        saved_gripper=gateway._grippers.pop(gateway.active_arm,None)
+        try:
+            legacy=Float32MultiArray();legacy.data=[1.,1.,300.,300.,33.,0.]
+            gateway._legacy_grip(legacy)
+            assert gateway.states()[1][gateway.active_arm]['connected'] is False
+        finally:
+            if saved_gripper:gateway._grippers[gateway.active_arm]=saved_gripper
         from arm_msgs.msg import TactileStatus
         from station import site_catalog
         force=TactileStatus();force.connected=True;force.fnormal=1.25;force.fshearx=-.5;force.fsheary=.75
         gateway._tactile('left',force)
         assert site_catalog._TACTILE['left']['fnormal']==1.25
         assert site_catalog._TACTILE['left']['fshearx']==-.5
+        from geometry_msgs.msg import WrenchStamped
+        calibrated=WrenchStamped();calibrated.wrench.force.x=.2;calibrated.wrench.force.y=-.3;calibrated.wrench.force.z=.4
+        gateway._tactile_wrench('left',calibrated);gateway._tactile('left',force)
+        assert site_catalog._TACTILE['left']['force_n']=={'fnormal':.4,'fshearx':.2,'fsheary':-.3}
+        assert site_catalog._TACTILE['left']['fnormal']==1.25
+        assert gateway.tactile_history()['left'][-1]['fshearx']==.2
+        packet=gateway.tactile_sample_packet({})
+        assert packet['samples']['left'][-1]['fshearx']==.2
+        seq=packet['samples']['left'][-1]['seq']
+        assert gateway.tactile_sample_packet({'left':seq})['samples']['left']==[]
+        gateway.clear_tactile_reference('left')
+        assert gateway.tactile_history()['left']==[]
+        assert gateway.tactile_sample_packet({})['epochs']['left']>packet['epochs']['left']
+        assert site_catalog._TACTILE['left']['force_n'] is None
         gateway.watch_tactile(True)
         gateway._tactile_image('left_thermal',message)
         assert 'left_thermal' in gateway.tactile_frames()