cheng
97 lines
5.2 KiB
Python
97 lines
5.2 KiB
Python
"""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()
|