Update workstation UI, ROS task control and device integration

station/test/device_nodes_fixture.py

@@ -0,0 +1,96 @@
+"""Run new device nodes on a private ROS domain, with every device mocked."""
+import threading
+import time
+from unittest.mock import patch
+import cv2
+import numpy as np
+import rclpy
+from rclpy.node import Node
+from rclpy.executors import MultiThreadedExecutor
+from rclpy.qos import qos_profile_sensor_data
+from sensor_msgs.msg import CompressedImage
+from geometry_msgs.msg import WrenchStamped
+from arm_msgs.msg import TactileStatus
+from arm_msgs.srv import GripperExecute
+from std_srvs.srv import Trigger
+from arm_application.dh_pgea import GripperState
+import arm_application.gripper_node as gripper
+from arm_application.tactile_flow_node import TactileFlowNode
+
+
+class FakeGripper:
+    writes=[];fail_force=False
+    def __init__(self,**kwargs):self.state=GripperState(init_state=1,grip_state=1,position=300)
+    def open(self):pass
+    def close(self):pass
+    def get_run_states(self,**kwargs):return self.state
+    def set_target_force(self,v):self.writes.append(('force',v));return not self.fail_force
+    def set_target_speed(self,v):self.writes.append(('speed',v));return True
+    def set_target_position(self,v):self.writes.append(('position',v));self.state.position=v;return True
+
+rclpy.init(args=['--ros-args','-r','__ns:=/isolated_devices'])
+gripper.DhPgeaGripper=FakeGripper
+jaw=gripper.GripperNode()
+with patch.object(TactileFlowNode,'_capture',return_value=None):sensor=TactileFlowNode()
+client_node=Node('device_test_client');statuses=[];frames=[];forces=[]
+client_node.create_subscription(TactileStatus,'/tactile/left/status',statuses.append,10)
+client_node.create_subscription(WrenchStamped,'/tactile/left/wrench',forces.append,qos_profile_sensor_data)
+client_node.create_subscription(CompressedImage,'/tactile/left/flow/compressed',frames.append,qos_profile_sensor_data)
+client=client_node.create_client(GripperExecute,'/isolated_devices/execute')
+executor=MultiThreadedExecutor(num_threads=2)
+for node in (jaw,sensor,client_node):executor.add_node(node)
+thread=threading.Thread(target=executor.spin,daemon=True);thread.start()
+try:
+    assert client.wait_for_service(timeout_sec=3)
+    def request(command,pos=0.,force=33.,speed=25.):
+        req=GripperExecute.Request();req.command.command=command;req.command.position=pos;req.command.force=force;req.command.speed=speed
+        future=client.call_async(req);end=time.monotonic()+3
+        while not future.done() and time.monotonic()<end:time.sleep(.01)
+        assert future.done();return future.result()
+    assert request('connect').success and FakeGripper.writes==[]
+    assert not request('init').success and FakeGripper.writes==[]
+    assert not request('move',float('nan')).success and FakeGripper.writes==[]
+    assert request('move',7.,25.,12.).success
+    assert FakeGripper.writes==[('force',25),('speed',12),('position',350)]
+    FakeGripper.writes=[];FakeGripper.fail_force=True
+    assert not request('move',5.).success
+    assert FakeGripper.writes==[('force',33)]
+    ok,encoded=cv2.imencode('.jpg',np.zeros((64,64,3),np.uint8));assert ok
+    end=time.monotonic()+3
+    while time.monotonic()<end and not frames:
+        with sensor._lock:
+            sensor._sample=(time.monotonic(),dict(fps=30.,fnormal=123.,fshearx=36000.,fsheary=-36000.,contact=True))
+            sensor._error='';sensor._jpeg=(time.monotonic(),encoded.tobytes())
+        time.sleep(.02)
+    assert frames and any(s.connected and s.fnormal==123. for s in statuses)
+    end=time.monotonic()+2
+    while not forces and time.monotonic()<end:time.sleep(.01)
+    assert forces and abs(forces[-1].wrench.force.x-1.19)<1e-6
+    assert abs(forces[-1].wrench.force.y+1.19)<1e-6
+    assert forces[-1].wrench.force.z==0.
+    assert cv2.imdecode(np.frombuffer(frames[-1].data,np.uint8),cv2.IMREAD_COLOR).shape==(64,64,3)
+    reset_client=client_node.create_client(Trigger,'/tactile/left/reset_reference')
+    assert reset_client.wait_for_service(timeout_sec=2)
+    resets=[]
+    class FakeSensor:
+        def reset(self):resets.append('reset')
+    def reset_worker():
+        if sensor._reference_reset.wait(1.):sensor._apply_reference_reset(FakeSensor())
+    with sensor._lock:sensor._sample=(time.monotonic(),dict(fps=30.,fnormal=0.,fshearx=0.,fsheary=0.,contact=False))
+    worker=threading.Thread(target=reset_worker);worker.start()
+    future=reset_client.call_async(Trigger.Request());end=time.monotonic()+3
+    while not future.done() and time.monotonic()<end:time.sleep(.01)
+    worker.join(2)
+    assert future.done() and future.result().success and resets==['reset']
+    sensor._reference_reset.set();sensor._abort_reference_reset()
+    sensor._apply_reference_reset(FakeSensor())
+    assert resets==['reset'] and not sensor._reference_result[0]
+    with sensor._lock:sensor._sample=None;sensor._jpeg=None;sensor._error='USB offline'
+    end=time.monotonic()+2
+    while time.monotonic()<end and not any(not s.connected and s.error_msg=='USB offline' for s in statuses):time.sleep(.02)
+    assert any(not s.connected and s.error_msg=='USB offline' for s in statuses)
+    print('new gripper typed control, no-auto-init, failed-force guard, tactile JPEG and offline status PASS',flush=True)
+finally:
+    executor.shutdown(timeout_sec=3);thread.join(timeout=3)
+    for node in (jaw,sensor,client_node):node.destroy_node()
+    rclpy.shutdown()