Update workstation UI, ROS task control and device integration
This commit is contained in:
cheng
@@ -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()
|
||||
@@ -12,11 +12,47 @@ assert app.property('darkTheme') is True
|
||||
assert window._theme_selector.currentData()=='dark'
|
||||
window._apply_theme('light',persist=False)
|
||||
assert app.property('darkTheme') is False
|
||||
# A first status message and its expiry must not resize the dashboard.
|
||||
app.processEvents()
|
||||
content_before=window.centralWidget().geometry()
|
||||
window.statusBar().showMessage('刷新相机 · 状态提示 '*30)
|
||||
app.processEvents()
|
||||
assert window.centralWidget().geometry()==content_before
|
||||
window.statusBar().clearMessage();app.processEvents()
|
||||
assert window.centralWidget().geometry()==content_before
|
||||
assert window._theme_selector.currentData()=='light'
|
||||
assert 'rclpy' not in sys.modules
|
||||
assert 'serial' not in sys.modules
|
||||
assert 'station.operator_console' not in sys.modules
|
||||
assert set(window._pages) == {'main','calibration','settings','ros_monitor'}
|
||||
# Offline display uses the configured HOME, while real feedback always wins.
|
||||
assert window._home_poses[window._arm] != [0.]*7
|
||||
assert window._arm_sketch._pose_key[0] == tuple(round(q,3) for q in window._home_poses[window._arm])
|
||||
assert not window._arm_sketch._live
|
||||
from unittest.mock import patch
|
||||
live_snap=dict(window._snap)
|
||||
live_snap['arm_live']={window._arm:{'pose_live':True,'q':[.12]*7}}
|
||||
with patch.object(ui,'status',return_value=live_snap):
|
||||
window._refresh()
|
||||
assert window._arm_sketch._pose_key[0]==(.12,)*7
|
||||
assert window._arm_sketch._live
|
||||
window._refresh()
|
||||
# Calibration follows arm/mode and shares the existing camera preview path.
|
||||
from unittest.mock import patch
|
||||
window._show_page('calibration')
|
||||
window._snap['camera_ownership']=[{'id':cam['id'],'owned':True} for cam in ui.camera_view.targets()]
|
||||
with patch.object(ui.camera_view, 'watch') as watch:
|
||||
window._calibration_mode.setCurrentIndex(0)
|
||||
window._sync_camera_watch()
|
||||
wrist=window._calibration_camera()
|
||||
assert wrist and wrist['arm']==window._arm
|
||||
assert wrist['id']+'_color' in watch.call_args.args[0]
|
||||
window._calibration_mode.setCurrentIndex(1)
|
||||
outside=window._calibration_camera()
|
||||
assert outside and not outside.get('arm')
|
||||
assert outside['id']+'_color' in watch.call_args.args[0]
|
||||
window._calibration_mode.setCurrentIndex(0)
|
||||
window._show_page('main')
|
||||
# A photo must preserve the current RGB frame; no capture/device operation is needed.
|
||||
import tempfile
|
||||
from unittest.mock import patch
|
||||
@@ -34,6 +70,66 @@ with tempfile.TemporaryDirectory() as pictures:
|
||||
assert selected['id'] in window._camera_windows
|
||||
window._camera_windows[selected['id']].close()
|
||||
assert not window._camera_windows
|
||||
# Device controls only submit explicit actions; changing settings never writes to hardware.
|
||||
assert not hasattr(window,'_grip_force') and not hasattr(window,'_grip_target')
|
||||
assert window._gripper_probe_button.text()=='连接 / 探测'
|
||||
assert window._sensor_refresh_button.text()=='刷新传感器'
|
||||
assert window._sensor_reset_button.text()=='重置底图'
|
||||
assert window._sensor_reset_button.size()==window._sensor_refresh_button.size()
|
||||
assert window._gripper_probe_button.size()==window._sensor_refresh_button.size()
|
||||
assert window._gripper_probe_button.height()==28
|
||||
original_arm=window._arm
|
||||
window._set_arm('91')
|
||||
assert window._gripper_sketch._cad_static and window._gripper_sketch._gl_count>0
|
||||
assert window._gripper_sketch._caption()==''
|
||||
window._set_arm(original_arm)
|
||||
assert not window._gripper_sketch._cad_static
|
||||
assert window._gripper_sketch._caption()==''
|
||||
from PySide6.QtCore import QByteArray,QBuffer,QIODevice
|
||||
sample_image=ui.QImage(32,32,ui.QImage.Format.Format_RGB888);sample_image.fill(ui.QColor('#1683ff'))
|
||||
encoded=QByteArray();buffer=QBuffer(encoded);buffer.open(QIODevice.OpenModeFlag.WriteOnly)
|
||||
assert sample_image.save(buffer,'JPG');buffer.close()
|
||||
fake_snap=dict(window._snap)
|
||||
fake_snap['backend']={**fake_snap.get('backend',{}),'error':''}
|
||||
fake_snap['arm_live']={original_arm:{'tcp_pose':[500.,120.,325.,179.2,.5,88.1]}}
|
||||
fake_snap['tactile']={'left':{'connected':True,'fps':29.,'fnormal':12000.,'fshearx':20000.,'fsheary':-10000.,
|
||||
'force_n':{'fnormal':.477,'fshearx':.65,'fsheary':-.31}}}
|
||||
fake_snap['history_n']={'left':[{'t':time.time()-.1,'fnormal':.477,'fshearx':.65,'fsheary':-.31}]}
|
||||
with patch.object(ui,'status',return_value=fake_snap),patch.object(ui.runtime.tactile_view,'frame',return_value=bytes(encoded)):
|
||||
window._refresh();window._paint_images()
|
||||
assert [v.text() for v in window._tcp_values]==['500.0','120.0','325.0','179.2','0.5','88.1']
|
||||
assert '(29fps)' in window._flow_slots[0][0].text()
|
||||
assert '·' not in window._flow_slots[0][0].text()
|
||||
assert '当前法向力 0.48N' in window._flow_slots[0][0].text()
|
||||
assert not hasattr(window,'_flow_details')
|
||||
assert window._flow_axis_charts['left']['x']._samples[-1]['fshearx']==.65
|
||||
assert window._flow_axis_charts['left']['y']._samples[-1]['fsheary']==-.31
|
||||
assert window._flow_axis_charts['left']['x']._scale==window._flow_axis_charts['left']['y']._scale==10.
|
||||
assert window._flow_axis_charts['left']['x'].MAX_SAMPLES==150
|
||||
app.processEvents()
|
||||
for triplet in window._flow_triplets.values():
|
||||
rectangles=[item.geometry() for item in triplet.items]
|
||||
assert all(rect.width()==rect.height() for rect in rectangles),rectangles
|
||||
assert len({rect.width() for rect in rectangles})==1,rectangles
|
||||
assert len({rect.y() for rect in rectangles})==1,rectangles
|
||||
assert not window._flow_slots[0][1]._source.isNull()
|
||||
# Recent samples are spread across a shorter window without altering source data.
|
||||
chart=window._flow_axis_charts['left']['x']
|
||||
history=[{'seq':i,'fshearx':.2,'fsheary':-.2} for i in range(1800)]
|
||||
chart.set_samples(history)
|
||||
assert len(chart._samples)==150 and chart._samples[0]['seq']==1650
|
||||
assert history[-1]['fshearx']==.2
|
||||
assert ui._display_force(.299)==ui._display_force(-.299)==0.
|
||||
assert ui._display_force(.3)==.3 and ui._display_force(-.3)==-.3
|
||||
assert window._flow_slots[0][0].textFormat()==ui.Qt.TextFormat.PlainText
|
||||
title=window._flow_slots[0][0]
|
||||
triplet=window._flow_triplets['left']
|
||||
assert title.geometry().top()>=triplet.geometry().bottom()
|
||||
fake_snap['history_n']['left'][-1]['fnormal']=.25
|
||||
window._refresh();window._paint_images()
|
||||
assert '当前法向力 0N' in title.text()
|
||||
|
||||
|
||||
errors=[];ticks=[]
|
||||
duration=float(os.environ.get("ORISYS_GUI_TEST_SECONDS","2.2"))
|
||||
started=time.monotonic()
|
||||
@@ -50,6 +146,20 @@ original=sys.excepthook
|
||||
def fail(*args): errors.append(str(args[1]));original(*args)
|
||||
sys.excepthook=fail
|
||||
beat=ui.QTimer();beat.timeout.connect(tick);beat.start(50)
|
||||
# The GUI must not serialize read-only requests behind a long arm operation.
|
||||
parallel=[]
|
||||
def fake_command(action,aid,*args,**kwargs):
|
||||
parallel.append((action,time.monotonic()))
|
||||
if action=='stage_track':
|
||||
time.sleep(.65)
|
||||
parallel.append(('motion_finished',time.monotonic()))
|
||||
return {'ok':True,'message':'isolated test'}
|
||||
ui.run_action=fake_command
|
||||
ui.stop_started=lambda *args:(parallel.append(('stop',time.monotonic())),{'ok':True,'message':'isolated stop'})[1]
|
||||
ui.QTimer.singleShot(220,lambda:window._ask('stage_track'))
|
||||
ui.QTimer.singleShot(260,lambda:window._ask('handeye_show'))
|
||||
ui.QTimer.singleShot(280,lambda:window._ask('stage_track'))
|
||||
ui.QTimer.singleShot(300,window._stop_arm)
|
||||
ui.QTimer.singleShot(200,lambda:window._start(lambda:(time.sleep(.7),{'ok':True,'message':'test'})[1],'test'))
|
||||
ui.QTimer.singleShot(500,lambda:window._show_page('calibration'))
|
||||
ui.QTimer.singleShot(1000,lambda:window._show_page('calibration'))
|
||||
@@ -59,6 +169,23 @@ ui.QTimer.singleShot(round(duration*1000),window.close)
|
||||
ui.QTimer.singleShot(round((duration+15)*1000),app.quit)
|
||||
app.exec()
|
||||
assert len(ticks)>20,len(ticks)
|
||||
assert [name for name,_ in parallel].count('stage_track')==1,parallel
|
||||
assert dict(parallel)['handeye_show']<dict(parallel)['motion_finished'],parallel
|
||||
assert dict(parallel)['stop']<dict(parallel)['motion_finished'],parallel
|
||||
text = window._detail.toPlainText()
|
||||
assert '[点击] 检测跟踪' in text,text
|
||||
assert '[未重复发送]' in text,text
|
||||
assert 'isolated test' in text,text
|
||||
# Repeated refreshes cannot duplicate backend lifecycle events.
|
||||
window._snap['operation_log']={'session':'log-test','events':[
|
||||
{'seq':1,'time':time.time(),'action':'stage_track','arm':'89','task_id':'123456789','message':'收到操作请求'},
|
||||
{'seq':2,'time':time.time(),'action':'stage_track','arm':'89','task_id':'123456789','message':'识别阶段'}]}
|
||||
window._sync_operation_log();window._sync_operation_log()
|
||||
assert window._detail.toPlainText().count('识别阶段')==1
|
||||
assert '#12345678' in window._detail.toPlainText()
|
||||
window._ran({'ok':False,'operation':'对孔测试','message':'执行失败','detail':'测试用内部错误明细','log':'/tmp/isolated-motion.log'})
|
||||
assert '测试用内部错误明细' in window._detail.toPlainText()
|
||||
assert '/tmp/isolated-motion.log' in window._detail.toPlainText()
|
||||
assert not errors,errors
|
||||
assert runtime._runtime is None
|
||||
assert not any(job.isRunning() for job in window._jobs.values())
|
||||
|
||||
@@ -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()
|
||||
|
||||
@@ -0,0 +1,24 @@
|
||||
from unittest import TestCase
|
||||
from arm_application.camera_profiles import depth_profile
|
||||
from arm_application.realsense_color_pub import stream_open_passes, usb2_needs_reset
|
||||
|
||||
|
||||
class CameraProfilesTest(TestCase):
|
||||
def test_1080p_color_keeps_native_720p_depth_at_same_rate(self):
|
||||
modes={(1280,720,30),(848,480,30),(640,480,15)}
|
||||
self.assertEqual(depth_profile((1920,1080,30),modes),(1280,720,30))
|
||||
self.assertEqual(depth_profile((848,480,30),modes),(848,480,30))
|
||||
with self.assertRaises(ValueError): depth_profile((1920,1080,60),modes)
|
||||
|
||||
def test_usb2_enumeration_requests_one_hardware_reset(self):
|
||||
self.assertTrue(usb2_needs_reset('2.1'))
|
||||
self.assertTrue(usb2_needs_reset('2.0'))
|
||||
self.assertFalse(usb2_needs_reset('3.2'))
|
||||
self.assertFalse(usb2_needs_reset(''))
|
||||
|
||||
def test_depth_is_exhausted_before_color_only_fallback(self):
|
||||
names = [name for name, _modes in stream_open_passes(True, False, False)]
|
||||
self.assertEqual(names, ['depth', 'color'])
|
||||
depth_modes = next(modes for name, modes in stream_open_passes(True, False, True) if name == 'depth')
|
||||
self.assertEqual(depth_modes, ((True, 'none'),))
|
||||
self.assertTrue(all(with_depth for with_depth, _infra in depth_modes))
|
||||
@@ -10,10 +10,14 @@ class ClientStateTest(TestCase):
|
||||
client=RuntimeClient.__new__(RuntimeClient)
|
||||
client._lock=threading.RLock();client.error=''
|
||||
client._snapshot={'now':time.time()-10,'arm_live':{'89':{'enabled':True,'pose_live':True}},
|
||||
'gripper_live':{'89':{'connected':True,'pose_live':True}}}
|
||||
'gripper_live':{'89':{'connected':True,'pose_live':True}},
|
||||
'backend':{'monitoring':{'devices':[{'health':'live','enabled':True}],
|
||||
'processes':[{'state':'running','running':True}]}}}
|
||||
with patch('station.rpc.call',side_effect=AssertionError('paint path must not perform RPC')):
|
||||
snapshot=client.snapshot()
|
||||
self.assertFalse(snapshot['arm_live']['89']['pose_live'])
|
||||
self.assertIsNone(snapshot['arm_live']['89']['enabled'])
|
||||
self.assertFalse(snapshot['gripper_live']['89']['connected'])
|
||||
self.assertEqual(snapshot['backend']['monitoring']['devices'][0]['health'],'unknown')
|
||||
self.assertEqual(snapshot['backend']['monitoring']['processes'][0]['state'],'unknown')
|
||||
self.assertTrue(client._snapshot['gripper_live']['89']['connected'])
|
||||
|
||||
@@ -0,0 +1,21 @@
|
||||
import hashlib
|
||||
import json
|
||||
from pathlib import Path
|
||||
from unittest import TestCase
|
||||
import numpy as np
|
||||
from station.operator_gripper_mesh import rgi_cad_gl
|
||||
|
||||
|
||||
class GripperCadTest(TestCase):
|
||||
def test_tessellation_is_from_supplied_step_and_units_are_metres(self):
|
||||
folder=Path(__file__).resolve().parents[1]/'src/station/assets/models/rgi-100-30'
|
||||
meta=json.loads((folder/'mesh.json').read_text())
|
||||
self.assertEqual(hashlib.sha256((folder/'source.step').read_bytes()).hexdigest(),meta['source_sha256'])
|
||||
self.assertEqual(meta['source_units'],'mm');self.assertEqual(meta['units'],'m')
|
||||
self.assertFalse(meta['articulated'])
|
||||
blob,count,center,radius=rgi_cad_gl()
|
||||
vertices=np.frombuffer(blob,np.float32).reshape(-1,6)
|
||||
self.assertEqual(count,meta['triangles']*3);self.assertEqual(len(vertices),count)
|
||||
self.assertTrue(np.isfinite(vertices).all());self.assertGreater(radius,.01)
|
||||
self.assertTrue(np.allclose(np.ptp(vertices[:,:3],axis=0)*1000,meta['dimensions_mm'],atol=.001))
|
||||
self.assertLess(np.max(np.abs(vertices[:,:3])),1.)
|
||||
@@ -8,27 +8,40 @@ from station.control_service import StationService
|
||||
class Gateway:
|
||||
active_arm='89'
|
||||
def __init__(self): self.started=None;self.requests=[]
|
||||
def graph(self): return {'nodes':[],'topics':[],'services':['/station/grippers/arm_89/execute']}
|
||||
def graph(self):
|
||||
return {'nodes':['dh_pgea'],'topics':['/gripper/states'],
|
||||
'services':['/gripper/open','/gripper/close','/gripper/initialize','/dh_pgea/execute']}
|
||||
def publisher_count(self, topic): return 1 if topic=='/gripper/states' else 0
|
||||
def states(self):
|
||||
elapsed=time.monotonic()-self.started if self.started else 0
|
||||
return {},{'89':{'connected':True,'moving':bool(self.started and elapsed<.25),
|
||||
'grip_state':0 if self.started and elapsed<.25 else 1,'permille':500.}}
|
||||
moving=bool(self.started and elapsed<getattr(self,'motion_duration',.05))
|
||||
last=self.requests[-1][0] if self.requests else ''
|
||||
if last=='/gripper/close':
|
||||
permille=0. if not moving else 400.
|
||||
elif last=='/gripper/open':
|
||||
permille=1000. if not moving else 400.
|
||||
else:
|
||||
permille=500.
|
||||
return {},{'89':{'connected':True,'moving':moving,
|
||||
'grip_state':0 if moving else 1,'permille':permille,
|
||||
'gripped':False,'opening_mm':permille/1000.*20}}
|
||||
def call(self,typ,name,request,event,timeout):
|
||||
self.requests.append(request);self.started=time.monotonic()
|
||||
self.requests.append((name,request));self.started=time.monotonic()
|
||||
return type('Reply',(),{'success':True,'message':'accepted'})()
|
||||
def close(self): pass
|
||||
|
||||
|
||||
class GripperCommandsTest(TestCase):
|
||||
def test_waits_for_physical_completion_and_rejects_bounds(self):
|
||||
gateway=Gateway()
|
||||
gateway=Gateway();gateway.motion_duration=.25
|
||||
with tempfile.TemporaryDirectory() as root:
|
||||
service=StationService(root=root,gateway=gateway)
|
||||
try:
|
||||
arm=service.arms['89'];start=time.monotonic()
|
||||
result=service._gripper('gripper_tune',arm,{'kind':'pos','pos':500,'force':33,'speed':25},threading.Event())
|
||||
self.assertTrue(result['ok']);self.assertGreater(time.monotonic()-start,.2)
|
||||
self.assertEqual(gateway.requests[0].command.position,arm['stroke_mm']/2)
|
||||
self.assertEqual(gateway.requests[0][0],'/dh_pgea/execute')
|
||||
self.assertEqual(gateway.requests[0][1].command.position,arm['stroke_mm']/2)
|
||||
count=len(gateway.requests)
|
||||
with self.assertRaises(ValueError):
|
||||
service._gripper('gripper_tune',arm,{'kind':'pos','pos':500,'force':999,'speed':25},threading.Event())
|
||||
@@ -49,3 +62,73 @@ class GripperCommandsTest(TestCase):
|
||||
self.assertEqual(gateway.requests,[])
|
||||
self.assertTrue(service._frozen_config.is_file())
|
||||
finally: service.close()
|
||||
|
||||
def test_open_close_do_not_require_initialized(self):
|
||||
gateway=Gateway()
|
||||
original=gateway.states
|
||||
def states():
|
||||
arms,grips=original()
|
||||
row=dict(grips['89']);row['initialized']=False
|
||||
return arms,{'89':row}
|
||||
gateway.states=states
|
||||
with tempfile.TemporaryDirectory() as root:
|
||||
service=StationService(root=root,gateway=gateway)
|
||||
try:
|
||||
closed=service._gripper('gripper_close',service.arms['89'],{},threading.Event())
|
||||
self.assertTrue(closed['ok'])
|
||||
self.assertEqual(gateway.requests[-1][0],'/gripper/close')
|
||||
opened=service._gripper('gripper_open',service.arms['89'],{},threading.Event())
|
||||
self.assertTrue(opened['ok'])
|
||||
self.assertEqual(gateway.requests[-1][0],'/gripper/open')
|
||||
finally: service.close()
|
||||
|
||||
def test_need_init_summary_does_not_block_open_close(self):
|
||||
from station.operator_gripper_state import present
|
||||
row=present('89',rotary=False,stroke_mm=20.0,
|
||||
reading={'connected':True,'initialized':False,'permille':981,
|
||||
'grip_state':1,'source':'ROS'})
|
||||
self.assertIn('零点未标定', row['summary'])
|
||||
self.assertIn('开口 19.6 mm', row['summary'])
|
||||
self.assertNotIn('不会动', row['summary'])
|
||||
self.assertNotIn('请先点到位', row['summary'])
|
||||
|
||||
def test_open_close_init_call_dh_pgea_triggers_and_close_must_reach_zero(self):
|
||||
gateway=Gateway()
|
||||
with tempfile.TemporaryDirectory() as root:
|
||||
service=StationService(root=root,gateway=gateway)
|
||||
try:
|
||||
arm=service.arms['89']
|
||||
closed=service._gripper('gripper_close',arm,{},threading.Event())
|
||||
self.assertTrue(closed['ok'])
|
||||
self.assertEqual(gateway.requests[-1][0],'/gripper/close')
|
||||
opened=service._gripper('gripper_open',arm,{},threading.Event())
|
||||
self.assertTrue(opened['ok'])
|
||||
self.assertEqual(gateway.requests[-1][0],'/gripper/open')
|
||||
inited=service._gripper('gripper_init',arm,{},threading.Event())
|
||||
self.assertTrue(inited['ok'])
|
||||
self.assertEqual(gateway.requests[-1][0],'/gripper/initialize')
|
||||
finally: service.close()
|
||||
|
||||
def test_close_fails_when_firmware_says_arrived_but_opening_remains(self):
|
||||
gateway=Gateway()
|
||||
def states():
|
||||
return {},{'89':{'connected':True,'moving':False,'grip_state':1,'permille':130.,
|
||||
'gripped':False,'opening_mm':2.6}}
|
||||
gateway.states=states
|
||||
with tempfile.TemporaryDirectory() as root:
|
||||
service=StationService(root=root,gateway=gateway)
|
||||
try:
|
||||
result=service._gripper('gripper_close',service.arms['89'],{},threading.Event())
|
||||
self.assertFalse(result['ok'])
|
||||
self.assertIn('未闭合到位', result['message'])
|
||||
self.assertIn('2.6', result['message'])
|
||||
finally: service.close()
|
||||
|
||||
def test_gripper_plan_starts_dh_pgea_without_auto_init(self):
|
||||
from station.launch_plans import gripper_plan
|
||||
argv=gripper_plan({'id':'89','rotary':False,'port':'/dev/ttyUSB0','stroke_mm':20.0,'gripper':'PGEA-100-20-W'},
|
||||
{'baud':115200,'force_pct':20,'speed_pct':50})
|
||||
joined=' '.join(str(part) for part in argv)
|
||||
self.assertIn('arm_application.dh_pgea_node', joined)
|
||||
self.assertIn('auto_init:=false', joined)
|
||||
self.assertNotIn('gripper_node', joined)
|
||||
|
||||
@@ -0,0 +1,34 @@
|
||||
"""Only commands writing the same device conflict; all functions are fake."""
|
||||
import threading
|
||||
import time
|
||||
from unittest import TestCase
|
||||
from station.command_policy import resources_for
|
||||
from station.tasks import TaskManager
|
||||
|
||||
|
||||
class ModuleIndependenceTest(TestCase):
|
||||
def test_four_modules_execute_concurrently(self):
|
||||
manager=TaskManager();release=threading.Event();started={}
|
||||
def operation(name,event):
|
||||
started[name].set();release.wait(2)
|
||||
return {'ok':True}
|
||||
try:
|
||||
tokens=[]
|
||||
for name in ('real_up','camera_refresh','gripper_open','tactile_refresh'):
|
||||
started[name]=threading.Event()
|
||||
tokens.append(manager.submit(name,resources_for(name,'89'),lambda event,name=name:operation(name,event)))
|
||||
self.assertTrue(all(flag.wait(1) for flag in started.values()))
|
||||
self.assertTrue(all(manager.snapshot(t)['state']=='running' for t in tokens))
|
||||
with self.assertRaises(RuntimeError):
|
||||
manager.submit('gripper_close',resources_for('gripper_close','89'),lambda event:{'ok':True})
|
||||
finally:
|
||||
release.set();manager.close()
|
||||
|
||||
def test_only_composite_a7_workflow_also_owns_gripper(self):
|
||||
grip=resources_for('gripper_open','89')
|
||||
for action in ('real_up','reset_home','teach_goto','stage_track','stage_locate'):
|
||||
self.assertFalse(resources_for(action,'89') & grip,action)
|
||||
for action in ('stage_screw','pipeline'):
|
||||
self.assertTrue(resources_for(action,'89') & grip,action)
|
||||
self.assertFalse(resources_for(action,'89') & resources_for('tactile_reset','89'),action)
|
||||
self.assertFalse(resources_for(action,'89') & resources_for('camera_refresh','89'),action)
|
||||
@@ -11,7 +11,7 @@ class Gateway:
|
||||
def __init__(self): self.calls=[];self.fail_stop=False
|
||||
def graph(self): return {'nodes': [], 'topics': [], 'services': []}
|
||||
def states(self): return {}, {}
|
||||
def require_arm(self, aid, event): self.calls.append(('require',aid))
|
||||
def require_arm(self, aid, event, *, resume=True): self.calls.append(('require',aid) if resume else ('check_only',aid))
|
||||
def move_joints(self, arm, joints, speed, event):
|
||||
self.calls.append(('home',arm['id'],list(joints),speed));return {'ok':True}
|
||||
def stop_arm(self, arm):
|
||||
@@ -25,6 +25,7 @@ class Gateway:
|
||||
class MotionCommandsTest(TestCase):
|
||||
def setUp(self):
|
||||
self.gateway=Gateway();self.service=StationService(gateway=self.gateway)
|
||||
self.service.site.pop('remote_camera_preview',None)
|
||||
def tearDown(self): self.service.close()
|
||||
def test_camera_refresh_checks_both_cameras_without_robot_control(self):
|
||||
with patch.object(self.service,'_camera',side_effect=[RuntimeError('D405 missing'),{'ok':True,'message':'ready'}]) as camera:
|
||||
@@ -38,19 +39,51 @@ class MotionCommandsTest(TestCase):
|
||||
with patch.object(self.gateway,'graph',return_value={'nodes':['controller_manager'],'topics':[],'services':[]}),patch.object(self.service.processes,'start') as start:
|
||||
result=self.service.execute('real_up','89','eye_in_hand',{},threading.Event())
|
||||
self.assertTrue(result['ok']);start.assert_not_called()
|
||||
self.assertEqual(self.gateway.calls,[('require','89')])
|
||||
self.assertEqual(self.gateway.calls,[('check_only','89')])
|
||||
self.assertIsNone(self.service._motion_arm)
|
||||
|
||||
def test_detection_and_alignment_are_separate_stages(self):
|
||||
commands=[]
|
||||
def run(key, argv, event, env, cwd):
|
||||
def run(key, argv, event, env, cwd, **kwargs):
|
||||
commands.append(argv);return {'ok':True}
|
||||
with patch.object(self.service,'_camera'),patch.object(self.service,'_environment',return_value={}),patch.object(self.service.processes,'run',side_effect=run):
|
||||
self.service.execute('stage_track','89','eye_in_hand',{},threading.Event())
|
||||
self.service.execute('stage_locate','89','eye_in_hand',{},threading.Event())
|
||||
self.assertIn('skip_stage1:=false',commands[0]);self.assertIn('skip_stage2:=true',commands[0])
|
||||
self.assertIn('skip_stage3:=true',commands[0]);self.assertIn('d435_move_only:=false',commands[0])
|
||||
self.assertIn('run_a7_after_insert:=false',commands[0]);self.assertIn('completion_linger_sec:=0.3',commands[0]);self.assertIn('skip_stage3:=true',commands[0]);self.assertIn('d435_move_only:=false',commands[0])
|
||||
self.assertIn('skip_stage1:=true',commands[1]);self.assertIn('skip_stage2:=false',commands[1])
|
||||
self.assertIn('no_descent:=true',commands[1]);self.assertIn('skip_stage3:=true',commands[1])
|
||||
self.assertIn('run_a7_after_insert:=false',commands[1]);self.assertIn('no_descent:=false',commands[1]);self.assertIn('skip_stage3:=true',commands[1])
|
||||
def test_rotation_is_a7_only_and_does_not_open_cameras(self):
|
||||
with patch.object(self.service,'_rotation_ready'),patch.object(self.service,'_camera') as camera, \
|
||||
patch.object(self.service,'_environment',return_value={}),patch.object(self.service.processes,'run',return_value={'ok':True}) as run:
|
||||
self.assertTrue(self.service.execute('stage_screw','89','eye_in_hand',{},threading.Event())['ok'])
|
||||
self.assertEqual(run.call_args.args[1][-2:],['-m','arm_application.a7_rotation_node'])
|
||||
camera.assert_not_called()
|
||||
|
||||
def test_full_flow_runs_rotation_only_after_alignment_success(self):
|
||||
from station.tasks import Cancelled
|
||||
for first_ok,cancelled,expected_runs in ((True,False,2),(False,False,1),(True,True,1)):
|
||||
event=threading.Event();commands=[]
|
||||
def run(key,argv,*args,**kwargs):
|
||||
commands.append(argv)
|
||||
if cancelled:event.set()
|
||||
return {'ok':first_ok}
|
||||
with patch.object(self.service,'_rotation_ready'),patch.object(self.service,'_camera'), \
|
||||
patch.object(self.service,'_environment',return_value={}),patch.object(self.service.processes,'run',side_effect=run):
|
||||
if cancelled:
|
||||
with self.assertRaises(Cancelled):self.service.execute('pipeline','89','eye_in_hand',{},event)
|
||||
else:self.service.execute('pipeline','89','eye_in_hand',{},event)
|
||||
self.assertEqual(len(commands),expected_runs)
|
||||
self.assertIn('no_descent:=false',commands[0])
|
||||
self.assertIn('run_a7_after_insert:=false',commands[0])
|
||||
if expected_runs==2:self.assertEqual(commands[1][-1],'arm_application.a7_rotation_node')
|
||||
|
||||
def test_rotation_requires_own_typed_gripper_before_any_task(self):
|
||||
with patch.object(self.service.processes,'run') as run:
|
||||
with self.assertRaisesRegex(RuntimeError,'平行夹爪'):
|
||||
self.service.execute('stage_screw','89','eye_in_hand',{},threading.Event())
|
||||
run.assert_not_called()
|
||||
|
||||
def test_home_uses_each_arms_named_initial_pose(self):
|
||||
import yaml
|
||||
for aid,arm in self.service.arms.items():
|
||||
@@ -72,3 +105,144 @@ class MotionCommandsTest(TestCase):
|
||||
self.assertTrue(self.service.stop_commands('89')['ok']);self.assertEqual(self.service._stop_error,'')
|
||||
def test_shutdown_cancel_does_not_stop_unrelated_controller(self):
|
||||
self.assertTrue(self.service.stop_commands()['ok']);self.assertEqual(self.gateway.calls,[])
|
||||
|
||||
def test_external_camera_is_not_adopted_or_started_again(self):
|
||||
env={'SITE_CAM_SERIAL':'test-serial','SITE_CAM_NAME':'d405_left'}
|
||||
with patch.object(self.service,'_environment',return_value=env), \
|
||||
patch.object(self.gateway,'publisher_count',return_value=1,create=True), \
|
||||
patch.object(self.gateway,'publisher_nodes',return_value=['foreign_camera'],create=True), \
|
||||
patch.object(self.service.processes,'start') as start:
|
||||
with self.assertRaisesRegex(RuntimeError,'外部'):
|
||||
self.service._camera(self.service.arms['89'],False,threading.Event())
|
||||
start.assert_not_called()
|
||||
def test_camera_process_without_frames_is_not_ready(self):
|
||||
with patch.object(self.service,'_camera_ownership',return_value={'owned':True,'conflict':False}), \
|
||||
patch.object(self.gateway,'camera_health',return_value={'fresh':False},create=True):
|
||||
with self.assertRaisesRegex(RuntimeError,'没有新帧'):
|
||||
self.service._ensure_camera('d405_left',[],{},threading.Event())
|
||||
|
||||
def test_camera_refresh_preserves_healthy_matching_stream(self):
|
||||
with patch.object(self.service,'_camera_ownership',return_value={'owned':True,'conflict':False}), \
|
||||
patch.object(self.gateway,'camera_health',return_value={'fresh':True,'width':1280,'height':720},create=True), \
|
||||
patch.object(self.gateway,'publisher_count',return_value=1,create=True), \
|
||||
patch.object(self.service.processes,'stop') as stop:
|
||||
result=self.service._ensure_camera('d405_left',['-p','width:=1280','-p','height:=720'],{},threading.Event(),refresh=True)
|
||||
self.assertTrue(result['ok']);stop.assert_not_called()
|
||||
|
||||
def test_camera_refresh_keeps_usb2_fallback_resolution(self):
|
||||
with patch.object(self.service,'_camera_ownership',return_value={'owned':True,'conflict':False}), \
|
||||
patch.object(self.gateway,'camera_health',return_value={'fresh':True,'width':640,'height':480},create=True), \
|
||||
patch.object(self.gateway,'publisher_count',return_value=1,create=True), \
|
||||
patch.object(self.service.processes,'stop') as stop:
|
||||
result=self.service._ensure_camera('d405_left',['-p','width:=1280','-p','height:=720'],{},threading.Event(),refresh=True)
|
||||
self.assertTrue(result['ok']);stop.assert_not_called()
|
||||
|
||||
def test_camera_refresh_reconnects_only_camera_even_while_arm_is_enabling(self):
|
||||
from unittest.mock import Mock
|
||||
self.service._motion_arm='89'
|
||||
active=[True]
|
||||
def snapshot():return {'camera:d405_left':{'running':active[0]}}
|
||||
def stop(key):
|
||||
self.assertEqual(key,'camera:d405_left');active[0]=False
|
||||
def start(*args):active[0]=True;return Mock(),Path('/tmp/fake-camera.log')
|
||||
with patch.object(self.service,'_camera_ownership',side_effect=[
|
||||
{'owned':True,'conflict':False}, {'owned':False,'conflict':False},
|
||||
{'owned':True,'conflict':False}]), \
|
||||
patch.object(self.gateway,'camera_health',side_effect=[{'fresh':False},{'fresh':True}],create=True), \
|
||||
patch.object(self.gateway,'clear_camera_health',create=True), \
|
||||
patch.object(self.gateway,'publisher_count',return_value=0,create=True), \
|
||||
patch.object(self.service.processes,'snapshot',side_effect=snapshot), \
|
||||
patch.object(self.service.processes,'stop',side_effect=stop), \
|
||||
patch.object(self.service.processes,'start',side_effect=start):
|
||||
self.assertTrue(self.service._ensure_camera('d405_left',[],{},threading.Event(),refresh=True)['ok'])
|
||||
self.assertEqual(self.gateway.calls,[])
|
||||
|
||||
def test_same_name_does_not_prove_camera_ownership(self):
|
||||
name='d405_left'
|
||||
with patch.object(self.gateway,'publisher_nodes',return_value=[self.service._camera_node_name(name)],create=True):
|
||||
self.assertFalse(self.service._camera_ownership(name,{})['owned'])
|
||||
self.assertTrue(self.service._camera_ownership(name,{'camera:'+name:{'running':True}})['owned'])
|
||||
with patch.object(self.gateway,'publisher_nodes',return_value=[self.service._camera_node_name(name),'foreign_camera'],create=True):
|
||||
self.assertFalse(self.service._camera_ownership(name,{'camera:'+name:{'running':True}})['owned'])
|
||||
|
||||
def test_independent_commands_can_run_during_tracking(self):
|
||||
import time
|
||||
gate=threading.Event()
|
||||
def execute(action,arm,mode,extras,event):
|
||||
if action=='stage_track': gate.wait(2)
|
||||
return {'ok':True}
|
||||
with patch.object(self.service,'execute',side_effect=execute):
|
||||
motion=self.service.submit('stage_track','89')
|
||||
try:
|
||||
with self.assertRaisesRegex(RuntimeError,'检测跟踪'):
|
||||
self.service.submit('reset_home','89')
|
||||
with self.assertRaisesRegex(RuntimeError,'检测跟踪'):
|
||||
self.service.submit('reset_home','91')
|
||||
tokens=[self.service.submit('gripper_open','89'),self.service.submit('tactile_reset','89'),
|
||||
self.service.submit('camera_refresh','89'),
|
||||
self.service.submit('handeye_show','89'),
|
||||
self.service.submit('gripper_open','91')]
|
||||
deadline=time.monotonic()+1
|
||||
while time.monotonic()<deadline and any(self.service.tasks.snapshot(t)['state'] in ('queued','running') for t in tokens): time.sleep(.01)
|
||||
self.assertTrue(all(self.service.tasks.snapshot(t)['state']=='succeeded' for t in tokens))
|
||||
self.assertIn(self.service.tasks.snapshot(motion)['state'],('queued','running'))
|
||||
finally: gate.set()
|
||||
|
||||
def test_feedback_interruption_cancels_task_before_stop_and_cleans_up(self):
|
||||
started=threading.Event(); release=threading.Event()
|
||||
def operation(event):
|
||||
started.set(); release.wait(2); return {'ok':True}
|
||||
token=self.service.tasks.submit('stage_track',{'robot-control'},operation,context={'arm':'89'})
|
||||
self.assertTrue(started.wait(1))
|
||||
def stop(arm):
|
||||
self.assertTrue(self.service.tasks.snapshot(token)['cancel_requested'])
|
||||
release.set(); return {'ok':True}
|
||||
with patch.object(self.gateway,'motion_interruption',return_value='feedback lost',create=True),patch.object(self.service,'stop_commands',side_effect=stop) as stopped:
|
||||
self.service._check_motion_interruption(); stopped.assert_called_once_with('89')
|
||||
self.assertEqual(self.service.error,'')
|
||||
self.assertEqual(self.service._arm_errors['89'],'feedback lost')
|
||||
import time
|
||||
deadline=time.monotonic()+2
|
||||
while self.service.tasks.snapshot(token)['state']=='running' and time.monotonic()<deadline: time.sleep(.01)
|
||||
self.assertEqual(self.service.tasks.snapshot(token)['state'],'cancelled')
|
||||
|
||||
def test_rejected_command_is_logged_before_returning_error(self):
|
||||
with self.assertRaisesRegex(ValueError,'未注册'):
|
||||
self.service.submit('invalid_action','89')
|
||||
events=self.service.snapshot()['operation_log']['events']
|
||||
own=[r for r in events if r['action']=='invalid_action']
|
||||
self.assertEqual([r['kind'] for r in own],['received','rejected'])
|
||||
self.assertIn('未注册',own[-1]['message'])
|
||||
|
||||
def test_sensor_refresh_restarts_only_owned_sensor_nodes(self):
|
||||
from unittest.mock import Mock
|
||||
records={'tactile:left':{'running':True},'tactile:right':{'running':True},
|
||||
'stack:89':{'running':True},'gripper:89':{'running':True}}
|
||||
with patch.object(self.service.processes,'snapshot',return_value=records), \
|
||||
patch.object(self.service.processes,'stop') as stop, \
|
||||
patch.object(self.service.processes,'start') as start, \
|
||||
patch.object(self.service,'_environment',return_value={}), \
|
||||
patch.object(self.gateway,'publisher_nodes',return_value=[],create=True), \
|
||||
patch.object(self.gateway,'watch_tactile',create=True):
|
||||
result=self.service.execute('tactile_refresh','89','eye_in_hand',{},threading.Event())
|
||||
self.assertTrue(result['ok'])
|
||||
self.assertEqual([c.args[0] for c in stop.call_args_list],['tactile:left','tactile:right'])
|
||||
self.assertEqual(start.call_count,2)
|
||||
with patch.object(self.service,'execute',return_value={'ok':True}):
|
||||
token=self.service.submit('tactile_refresh','89')
|
||||
self.assertIsNotNone(self.service.tasks.snapshot(token))
|
||||
|
||||
def test_reference_reset_calls_only_owned_sensor_service_without_restarting(self):
|
||||
reply=type('Reply',(),{'success':True,'message':'reference reset'})()
|
||||
with patch.object(self.service.processes,'snapshot',return_value={'tactile:left':{'running':True}}), \
|
||||
patch.object(self.service.processes,'stop') as stop, \
|
||||
patch.object(self.service.processes,'start') as start, \
|
||||
patch.object(self.gateway,'call',return_value=reply,create=True) as call, \
|
||||
patch.object(self.gateway,'clear_tactile_reference',create=True) as clear:
|
||||
result=self.service.execute('tactile_reset','89','eye_in_hand',{},threading.Event())
|
||||
self.assertTrue(result['ok']);self.assertIn('right',result['message'])
|
||||
self.assertEqual(call.call_args.args[1],'/tactile/left/reset_reference')
|
||||
clear.assert_called_once_with('left');start.assert_not_called();stop.assert_not_called()
|
||||
from station.command_policy import resources_for
|
||||
for action in ('stage_track','stage_locate','stage_screw','pipeline'):
|
||||
self.assertFalse(resources_for('tactile_reset','89') & resources_for(action,'89'))
|
||||
|
||||
@@ -0,0 +1,18 @@
|
||||
from unittest import TestCase
|
||||
from station.node_inventory import inventory, node_group
|
||||
|
||||
|
||||
class NodeInventoryTest(TestCase):
|
||||
def test_namespaces_duplicates_and_discovery_do_not_imply_ownership_or_health(self):
|
||||
result = inventory({'node_paths':['/left/servo_node','/right/servo_node','/left/servo_node','/_ros2cli_1']},
|
||||
{'motion:89':{'pid':123,'running':True,'owner':'station'}}, [],
|
||||
{'89':{'pose_live':False,'enabled':None}}, {})
|
||||
left = next(n for n in result['nodes'] if n['name']=='/left/servo_node')
|
||||
self.assertEqual(left['instances'],2)
|
||||
self.assertEqual(left['owner'],'unverified')
|
||||
self.assertEqual(result['duplicates'],['/left/servo_node'])
|
||||
self.assertEqual(result['devices'][0]['health'],'offline')
|
||||
self.assertEqual(result['processes'][0]['owner'],'station')
|
||||
self.assertEqual(node_group('/right/rizon_arm_controller'),'运动控制')
|
||||
self.assertEqual(node_group('/perception_node'),'算法任务')
|
||||
self.assertEqual(node_group('/realsense_color_pub'),'设备驱动')
|
||||
@@ -0,0 +1,35 @@
|
||||
import json
|
||||
import tempfile
|
||||
import threading
|
||||
import time
|
||||
from pathlib import Path
|
||||
from unittest import TestCase
|
||||
from station.operation_log import OperationLog
|
||||
from station.tasks import TaskManager
|
||||
|
||||
|
||||
class OperationLogTest(TestCase):
|
||||
def test_memory_bounded_but_disk_retains_every_event(self):
|
||||
with tempfile.TemporaryDirectory() as folder:
|
||||
log = OperationLog(Path(folder)/'events.jsonl', limit=2)
|
||||
for i in range(5): log.add('received', str(i), action='stage_track', arm='89')
|
||||
snap=log.snapshot()
|
||||
self.assertEqual([r['seq'] for r in snap['events']], [4,5])
|
||||
rows=[json.loads(line) for line in Path(snap['path']).read_text().splitlines()]
|
||||
self.assertEqual([r['seq'] for r in rows],list(range(1,6)))
|
||||
|
||||
def test_fast_task_keeps_all_transitions_between_gui_polls(self):
|
||||
manager=TaskManager()
|
||||
try:
|
||||
def execute(event):
|
||||
manager.set_phase(event,'识别目标')
|
||||
manager.set_phase(event,'识别目标')
|
||||
return {'ok':True,'message':'执行完成'}
|
||||
token=manager.submit('stage_track', {'robot-control'},execute,context={'arm':'89'})
|
||||
deadline=time.monotonic()+2
|
||||
while manager.snapshot(token)['state'] in ('queued','running') and time.monotonic()<deadline:
|
||||
time.sleep(.01)
|
||||
events=manager.events.snapshot()['events']
|
||||
self.assertEqual([r['kind'] for r in events],['accepted','running','phase','succeeded'])
|
||||
self.assertTrue(all(r['task_id']==token and r['arm']=='89' for r in events))
|
||||
finally: manager.close()
|
||||
@@ -0,0 +1,47 @@
|
||||
"""Compile/run the actual hardware guard without ROS or a robot connection."""
|
||||
import subprocess
|
||||
import tempfile
|
||||
from pathlib import Path
|
||||
from unittest import TestCase
|
||||
|
||||
class PositionCommandFenceTest(TestCase):
|
||||
def test_reject_old_targets_after_enable_and_native_movel(self):
|
||||
root = Path(__file__).resolve().parents[2]
|
||||
include = root / 'src/flexiv_ros2/flexiv_hardware/include'
|
||||
code = r"""
|
||||
#include <cassert>
|
||||
#include <limits>
|
||||
#include "flexiv_hardware/position_command_fence.hpp"
|
||||
int main() {
|
||||
flexiv_hardware::MotionExecutionLatch latch;
|
||||
assert(!latch.observe(true, true, false)); // not activated
|
||||
latch.activate(); assert(latch.observe(true, true, false));
|
||||
assert(!latch.observe(true, false, false)); // physical E-stop
|
||||
for (int i=0; i<2000; ++i) assert(!latch.observe(true, true, false)); // Enable cannot resume
|
||||
latch.activate(); assert(latch.observe(true, true, false));
|
||||
assert(!latch.observe(true, true, true)); // fault
|
||||
assert(!latch.observe(true, true, false)); // clear fault cannot resume
|
||||
latch.activate(); assert(!latch.observe(false, true, false)); // disconnect
|
||||
assert(!latch.observe(true, true, false));
|
||||
flexiv_hardware::PositionCommandFence fence;
|
||||
std::vector<double> current(7, .74), old(7, 0.);
|
||||
for (int i=0; i<2000; ++i) assert(!fence.accept(old, current));
|
||||
assert(!fence.accept({}, current));
|
||||
auto invalid = current; invalid[0] = std::numeric_limits<double>::quiet_NaN();
|
||||
assert(!fence.accept(invalid, current));
|
||||
assert(fence.accept(current, current));
|
||||
assert(fence.accept(old, current)); // normal trajectory after handshake
|
||||
fence.reset(); // native MoveL / controller activation
|
||||
for (int i=0; i<2000; ++i) assert(!fence.accept(old, current));
|
||||
auto start = current; start[0] += .0001;
|
||||
assert(fence.accept(start, current));
|
||||
assert(fence.accept(old, current));
|
||||
}
|
||||
"""
|
||||
with tempfile.TemporaryDirectory() as folder:
|
||||
source = Path(folder)/'fence.cpp'; binary = Path(folder)/'fence'
|
||||
source.write_text(code)
|
||||
built = subprocess.run(['g++','-std=c++17','-I',str(include),str(source),'-o',str(binary)],capture_output=True,text=True,timeout=30)
|
||||
self.assertEqual(built.returncode,0,built.stderr)
|
||||
result = subprocess.run([str(binary)],capture_output=True,text=True,timeout=5)
|
||||
self.assertEqual(result.returncode,0,result.stderr)
|
||||
@@ -25,3 +25,49 @@ class PreviewCacheTest(TestCase):
|
||||
preview._ingest()
|
||||
self.assertEqual(preview.previews(),{})
|
||||
self.assertIn('未更新',preview.state()['error'])
|
||||
|
||||
def test_unchanged_shared_frame_is_not_copied_again(self):
|
||||
from types import SimpleNamespace
|
||||
preview.watch({'left_color'},set())
|
||||
header=bytearray(preview._SHM_HEAD.size)
|
||||
preview._SHM_HEAD.pack_into(header,0,2,1280,720,1280*720*3)
|
||||
with tempfile.TemporaryDirectory() as folder:
|
||||
payload={'frames':{'left_color':{'name':'owned_test','store':'left_color','kind':'rgb'}},
|
||||
'meta':{},'ages':{'left_color':0},'exported_monotonic':time.monotonic()}
|
||||
(Path(folder)/'meta.json').write_text(json.dumps(payload))
|
||||
with patch.object(preview,'_BRIDGE_DIR',Path(folder)), \
|
||||
patch.object(preview,'_SHM_SEQ',{'owned_test':2}), \
|
||||
patch.object(preview,'_attach',return_value=SimpleNamespace(buf=header)), \
|
||||
patch.object(preview,'_shm_read',side_effect=AssertionError('unchanged frame copied')):
|
||||
preview._ingest()
|
||||
|
||||
def test_full_hd_is_preserved_and_larger_frames_fit_transport(self):
|
||||
import numpy as np
|
||||
image=np.zeros((1080,1920,3),np.uint8)
|
||||
self.assertEqual(preview.rgb_preview(image,bgr=False).shape,image.shape)
|
||||
portrait=preview.rgb_preview(np.zeros((1280,720,3),np.uint8),bgr=False)
|
||||
self.assertLessEqual(portrait.shape[0],1080)
|
||||
self.assertLessEqual(portrait.nbytes,preview._SHM_MAX)
|
||||
|
||||
def test_refresh_worker_exit_does_not_kill_preview(self):
|
||||
import os,sys,subprocess,threading
|
||||
real_popen=subprocess.Popen
|
||||
spawning=[]
|
||||
def popen(argv,**kwargs):
|
||||
spawning.append(threading.current_thread().name)
|
||||
command=[sys.executable,'-m','station.child_process',str(os.getpid()),
|
||||
sys.executable,'-c','import time;time.sleep(20)']
|
||||
proc=real_popen(command,**kwargs)
|
||||
time.sleep(.2) # Allow the child to install its parent-death watch.
|
||||
return proc
|
||||
with patch.object(preview,'stream_targets',return_value=[{'id':'fake','label':'fake'}]), \
|
||||
patch.object(preview.subprocess,'Popen',side_effect=popen), \
|
||||
patch.object(preview,'_poll_loop',side_effect=lambda:preview._STOP.wait(5)):
|
||||
worker=threading.Thread(target=preview.start,name='short-refresh-worker')
|
||||
worker.start();worker.join(3)
|
||||
try:
|
||||
self.assertFalse(worker.is_alive())
|
||||
self.assertEqual(spawning,['cam-poll'])
|
||||
time.sleep(.3)
|
||||
self.assertIsNone(preview._BRIDGE.poll())
|
||||
finally:preview.stop()
|
||||
|
||||
@@ -19,3 +19,43 @@ class ProcessesTest(TestCase):
|
||||
processes.run('long',[sys.executable,'-c','import time;time.sleep(30)'],event)
|
||||
self.assertFalse(processes.snapshot()['long']['running'])
|
||||
finally: processes.close()
|
||||
|
||||
def test_each_run_has_its_own_log_slice_and_explicit_owner(self):
|
||||
with tempfile.TemporaryDirectory() as directory:
|
||||
processes=ProcessSupervisor(directory)
|
||||
try:
|
||||
processes.run('tool:89',[sys.executable,'-c','print("OLD_FAILURE")'],threading.Event())
|
||||
result=processes.run('tool:89',[sys.executable,'-c','print("NEW_RESULT")'],threading.Event())
|
||||
self.assertIn('NEW_RESULT',result['detail']);self.assertNotIn('OLD_FAILURE',result['detail'])
|
||||
row=processes.snapshot()['tool:89']
|
||||
self.assertEqual(row['owner'],'station');self.assertEqual(row['state'],'exited')
|
||||
finally: processes.close()
|
||||
|
||||
def test_success_waits_for_orphaned_child_cleanup(self):
|
||||
from pathlib import Path
|
||||
from arm_application.process_lifecycle import _stat
|
||||
with tempfile.TemporaryDirectory() as directory:
|
||||
processes=ProcessSupervisor(directory)
|
||||
pidfile=Path(directory)/'pid'
|
||||
source="import subprocess,sys,time;from pathlib import Path;p=subprocess.Popen([sys.executable,'-c','import time;time.sleep(120)'],start_new_session=True);Path(sys.argv[1]).write_text(str(p.pid))"
|
||||
try:
|
||||
result=processes.run('motion:89',[sys.executable,'-c',source,str(pidfile)],threading.Event(),timeout=12)
|
||||
self.assertTrue(result['ok'],result)
|
||||
child=int(pidfile.read_text());state=_stat(child)
|
||||
self.assertTrue(state is None or state[2]=='Z','task reported success with a live orphan')
|
||||
finally: processes.close()
|
||||
|
||||
def test_process_cleanup_phase_is_reported_before_resource_release(self):
|
||||
from pathlib import Path
|
||||
with tempfile.TemporaryDirectory() as directory:
|
||||
processes=ProcessSupervisor(directory)
|
||||
ready=Path(directory)/'ready'
|
||||
child="import signal,time,sys;from pathlib import Path;signal.signal(signal.SIGINT,lambda *a:(time.sleep(.4),sys.exit(0)));Path(sys.argv[1]).write_text('ready');time.sleep(20)"
|
||||
source="import subprocess,sys,time;from pathlib import Path;subprocess.Popen([sys.executable,'-c',sys.argv[1],sys.argv[2]],start_new_session=True)\nwhile not Path(sys.argv[2]).exists():time.sleep(.01)"
|
||||
phases=[]
|
||||
try:
|
||||
result=processes.run('motion:89',[sys.executable,'-c',source,child,str(ready)],threading.Event(),timeout=8,on_phase=phases.append)
|
||||
self.assertTrue(result['ok'],result)
|
||||
self.assertIn('收尾',phases)
|
||||
self.assertFalse(processes.snapshot()['motion:89']['running'])
|
||||
finally: processes.close()
|
||||
|
||||
@@ -0,0 +1,53 @@
|
||||
import os
|
||||
import tempfile
|
||||
from pathlib import Path
|
||||
from types import SimpleNamespace
|
||||
from unittest import TestCase
|
||||
from unittest.mock import patch
|
||||
import threading
|
||||
|
||||
from station.remote_camera import settings,bridge_environment
|
||||
from station.operator_camera_view import stream_targets
|
||||
from station.control_service import StationService
|
||||
from arm_application.site_io import load_site_dict
|
||||
|
||||
|
||||
class RemotePreviewTest(TestCase):
|
||||
def site(self):
|
||||
site=load_site_dict();site['remote_camera_preview']={
|
||||
'enabled':True,'domain':0,'host':'192.168.2.10','interface_address':'192.168.2.50',
|
||||
'cameras':['d405_left','d435'],'transport':'raw'}
|
||||
return site
|
||||
|
||||
def test_only_explicit_remote_streams_use_raw(self):
|
||||
streams=stream_targets(self.site())
|
||||
self.assertEqual({s['id'] for s in streams},{'d405_left','d435'})
|
||||
self.assertTrue(all(s['preview_transport']=='raw' and s['source']=='remote' for s in streams))
|
||||
|
||||
def test_remote_environment_is_limited_to_preview_child(self):
|
||||
parent={'ROS_DOMAIN_ID':'42','ROS_LOCALHOST_ONLY':'1','REAL_ROS_DOMAIN_ID':'42',
|
||||
'ROS_DISCOVERY_SERVER':'some-other-server'}
|
||||
before=dict(parent)
|
||||
with tempfile.TemporaryDirectory() as folder:
|
||||
env=bridge_environment(self.site(),parent,folder)
|
||||
self.assertEqual(parent,before)
|
||||
self.assertEqual((env['ROS_DOMAIN_ID'],env['ROS_LOCALHOST_ONLY']),('0','0'))
|
||||
self.assertEqual(env['REAL_ROS_DOMAIN_ID'],'42')
|
||||
self.assertNotIn('ROS_DISCOVERY_SERVER',env)
|
||||
xml=Path(env['FASTRTPS_DEFAULT_PROFILES_FILE']).read_text()
|
||||
self.assertIn('<address>192.168.2.50</address>',xml)
|
||||
self.assertIn('<address>192.168.2.10</address>',xml)
|
||||
self.assertEqual(bridge_environment({},parent,'/unused'),parent)
|
||||
|
||||
def test_bad_address_is_rejected_before_creating_profile(self):
|
||||
site=self.site();site['remote_camera_preview']['host']='bad<xml>'
|
||||
with self.assertRaises(ValueError):settings(site)
|
||||
|
||||
def test_refresh_does_not_touch_acquisition_or_robot(self):
|
||||
gateway=SimpleNamespace(states=lambda:({},{}),graph=lambda:{'nodes':[],'topics':[],'services':[]},close=lambda:None)
|
||||
service=StationService(gateway=gateway);service.site.update(self.site())
|
||||
try:
|
||||
with patch.object(service,'_camera') as camera,patch.object(service.processes,'start') as start:
|
||||
result=service.execute('camera_refresh','89','eye_in_hand',{},threading.Event())
|
||||
self.assertTrue(result['ok']);camera.assert_not_called();start.assert_not_called()
|
||||
finally:service.close()
|
||||
@@ -0,0 +1,22 @@
|
||||
import os
|
||||
import subprocess
|
||||
from pathlib import Path
|
||||
from unittest import TestCase
|
||||
|
||||
|
||||
class RosDomainTest(TestCase):
|
||||
def policy(self, **changes):
|
||||
env={k:v for k,v in os.environ.items() if k not in ('REAL_ROS_DOMAIN_ID','REAL_ROS_LOCALHOST_ONLY')}
|
||||
env.update(changes)
|
||||
root=Path(__file__).resolve().parents[2]
|
||||
return subprocess.run(['bash','-c','source "$1"; ros_domain_force_real || exit $?; printf "%s %s" "$ROS_DOMAIN_ID" "$ROS_LOCALHOST_ONLY"','bash',str(root/'scripts/lib/ros_domain.sh')],env=env,capture_output=True,text=True,timeout=3)
|
||||
def test_inherited_global_and_simulation_domains_are_not_adopted(self):
|
||||
for inherited in ('0','77','123'):
|
||||
value=self.policy(ROS_DOMAIN_ID=inherited,ROS_LOCALHOST_ONLY='0')
|
||||
self.assertEqual(value.returncode,0,value.stderr)
|
||||
self.assertEqual(value.stdout,'42 1')
|
||||
def test_explicit_override_and_invalid_values(self):
|
||||
value=self.policy(REAL_ROS_DOMAIN_ID='231',REAL_ROS_LOCALHOST_ONLY='0')
|
||||
self.assertEqual(value.stdout,'231 0')
|
||||
for domain in ('-1','233','abc','77'):
|
||||
self.assertNotEqual(self.policy(REAL_ROS_DOMAIN_ID=domain).returncode,0)
|
||||
@@ -12,3 +12,10 @@ class RosInterfacesTest(TestCase):
|
||||
capture_output=True,text=True,timeout=25)
|
||||
self.assertEqual(result.returncode,0,result.stdout+'\n'+result.stderr)
|
||||
self.assertIn('dynamic camera subscriptions PASS',result.stdout)
|
||||
|
||||
def test_new_device_nodes_with_fake_hardware(self):
|
||||
env={**os.environ,'ROS_DOMAIN_ID':'232','ROS_LOCALHOST_ONLY':'1'}
|
||||
result=subprocess.run([sys.executable,str(Path(__file__).with_name('device_nodes_fixture.py'))],
|
||||
env=env,capture_output=True,text=True,timeout=20)
|
||||
self.assertEqual(result.returncode,0,result.stdout+'\n'+result.stderr)
|
||||
self.assertIn('offline status PASS',result.stdout)
|
||||
|
||||
@@ -0,0 +1,31 @@
|
||||
from pathlib import Path
|
||||
from unittest import TestCase
|
||||
from arm_application.tactile_calibration import ForceCurve,TactileCalibration
|
||||
|
||||
|
||||
class TactileCalibrationTest(TestCase):
|
||||
def setUp(self):
|
||||
path=Path(__file__).resolve().parents[2]/'src/arm_application/config/tactile/force_calibration.json'
|
||||
self.calibration=TactileCalibration(path)
|
||||
|
||||
def test_calibration_knots_units_and_shear_sign(self):
|
||||
self.assertAlmostEqual(self.calibration.normal.convert(10000),.377)
|
||||
self.assertAlmostEqual(self.calibration.normal.convert(30000),1.377)
|
||||
self.assertAlmostEqual(self.calibration.shear.convert(36000),1.19)
|
||||
self.assertAlmostEqual(self.calibration.shear.convert(-36000),-1.19)
|
||||
self.assertEqual(self.calibration.normal.convert(0),0.)
|
||||
self.assertEqual(self.calibration.shear.convert(-500),0.)
|
||||
|
||||
def test_interpolation_and_conversion_never_replace_raw_values(self):
|
||||
raw={'fnormal':20000.,'fshearx':52000.,'fsheary':-52000.}
|
||||
saved=dict(raw);force=self.calibration.convert(raw)
|
||||
self.assertAlmostEqual(force['fnormal'],.877)
|
||||
self.assertAlmostEqual(force['fshearx'],1.625)
|
||||
self.assertAlmostEqual(force['fsheary'],-1.625)
|
||||
self.assertEqual(raw,saved)
|
||||
with self.assertRaises(ValueError):self.calibration.normal.convert(float('nan'))
|
||||
|
||||
def test_invalid_or_disabled_curve_does_not_mislabel_raw_as_newtons(self):
|
||||
with self.assertRaises(ValueError):ForceCurve.from_config({'enabled':False})
|
||||
with self.assertRaises(ValueError):ForceCurve.from_config({'enabled':True,'method':'piecewise_linear',
|
||||
'points':[{'raw':1,'force_n':0},{'raw':1,'force_n':1}]})
|
||||
@@ -0,0 +1,60 @@
|
||||
import tempfile
|
||||
from pathlib import Path
|
||||
from types import SimpleNamespace
|
||||
from unittest import TestCase
|
||||
import numpy as np
|
||||
from arm_application.tactile_flow import tactile_devices,select_source,flow_preview,read_sample
|
||||
|
||||
|
||||
class TactileFlowTest(TestCase):
|
||||
def test_discovery_filters_realsense_and_duplicate_interfaces(self):
|
||||
with tempfile.TemporaryDirectory() as folder:
|
||||
root=Path(folder);sys=root/'class';sys.mkdir()
|
||||
for name,vendor,index in [('video0','8086','0'),('video1','1bcf','0'),('video2','1bcf','1'),('video3','0bda','0')]:
|
||||
entry=sys/name;entry.mkdir();(entry/'index').write_text(index)
|
||||
usb=root/('usb-'+name);usb.mkdir();(usb/'idVendor').write_text(vendor)
|
||||
(entry/'device').symlink_to(usb,target_is_directory=True)
|
||||
found=tactile_devices(sys,Path('/dev'))
|
||||
self.assertEqual(found,['/dev/video1'])
|
||||
self.assertEqual(select_source('left',{},found),'/dev/video1')
|
||||
self.assertIsNone(select_source('right',{},found))
|
||||
self.assertEqual(select_source('right',{'right_video':'/dev/v4l/by-path/sensor'},found),'/dev/v4l/by-path/sensor')
|
||||
|
||||
def test_actual_vector_field_changes_preview_and_rejects_bad_shapes(self):
|
||||
flow=np.zeros((64,64,2),np.float32)
|
||||
rest=flow_preview(flow);flow[...,0]=12
|
||||
motion=flow_preview(flow)
|
||||
self.assertEqual(motion.shape,(360,360,3));self.assertFalse(np.array_equal(rest,motion))
|
||||
with self.assertRaises(ValueError):flow_preview(np.zeros((8,8)))
|
||||
|
||||
def test_uses_raw_force_channels_and_reads_flow_only_once(self):
|
||||
info=SimpleNamespace(FPS=1,FNORMAL=2,FSHEARX=3,FSHEARY=4,FNORMAL_RAW=5,FSHEARX_RAW=6,FSHEARY_RAW=7,VRAW=8)
|
||||
calls=[]
|
||||
def read(*keys):
|
||||
calls.append(keys);return 30.,200.,3.,-4.,np.zeros((8,8,2))
|
||||
row=read_sample(SimpleNamespace(info=info,read_info=read))
|
||||
self.assertEqual(calls,[(1,5,6,7,8)])
|
||||
self.assertEqual(row['fnormal'],200.);self.assertEqual(row['fsheary'],-4.)
|
||||
|
||||
def test_preview_polling_is_independent_of_slow_state_poll(self):
|
||||
import base64,threading,time
|
||||
from unittest.mock import patch
|
||||
from station.runtime_client import RuntimeClient
|
||||
client=RuntimeClient.__new__(RuntimeClient)
|
||||
client._lock=threading.RLock();client._stop=threading.Event()
|
||||
from collections import deque
|
||||
client._force_history={'left':deque(maxlen=1800),'right':deque(maxlen=1800)}
|
||||
client._force_cursors={};client._force_epochs={}
|
||||
client._snapshot={'view':{'on':True}};client._frames={};client.socket=Path('/unused')
|
||||
calls=[]
|
||||
def rpc_call(*args,**kwargs):
|
||||
calls.append(args[1]);return {'frames':{'left_shear':base64.b64encode(b'jpeg').decode()},
|
||||
'samples':{'left':[{'seq':1,'t':time.time(),'fnormal':.5,'fshearx':.2,'fsheary':-.3}]},'epochs':{'left':0}}
|
||||
with patch('station.runtime_client.rpc.call',side_effect=rpc_call):
|
||||
thread=threading.Thread(target=client._poll_frames);thread.start()
|
||||
time.sleep(.16);client._stop.set();thread.join(1)
|
||||
self.assertGreaterEqual(len(calls),3)
|
||||
self.assertEqual(set(calls),{'tactile_preview'})
|
||||
self.assertEqual(client._frames['left_shear'],b'jpeg')
|
||||
self.assertEqual(len(client._force_history['left']),1)
|
||||
self.assertEqual(client._force_cursors['left'],1)
|
||||
@@ -17,7 +17,9 @@ class TasksTest(TestCase):
|
||||
|
||||
def test_resources_held_until_completion(self):
|
||||
gate=threading.Event()
|
||||
one=self.manager.submit('move',{'arm:89'},lambda cancel:(gate.wait(2),{'ok':True})[1])
|
||||
one=self.manager.submit('move',{'arm:89'},lambda cancel:(gate.wait(2),{'ok':True})[1],context={'arm':'89'})
|
||||
self.assertEqual(self.manager.snapshot(one)['resources'],['arm:89'])
|
||||
self.assertEqual(self.manager.snapshot(one)['context']['arm'],'89')
|
||||
with self.assertRaises(RuntimeError): self.manager.submit('move-again',{'arm:89'},lambda e:{'ok':True})
|
||||
other=self.manager.submit('camera',{'camera'},lambda e:{'ok':True})
|
||||
self.assertEqual(self.wait(other)['state'],'succeeded')
|
||||
@@ -33,3 +35,33 @@ class TasksTest(TestCase):
|
||||
token=self.manager.submit('reject',{'arm'},fail)
|
||||
result=self.wait(token)
|
||||
self.assertEqual(result['state'],'failed');self.assertIn('driver rejected',result['result']['message'])
|
||||
|
||||
def test_busy_message_identifies_cleanup_and_stop_cancels_only_motion(self):
|
||||
gate=threading.Event()
|
||||
motion=self.manager.submit('stage_locate',{'robot-control','arm:89'},lambda e:(gate.wait(2),{'ok':True})[1],context={'arm':'89'})
|
||||
camera=self.manager.submit('camera_refresh',{'camera-setup:89'},lambda e:(gate.wait(2),{'ok':True})[1])
|
||||
try:
|
||||
deadline=time.monotonic()+1
|
||||
while self.manager.snapshot(motion)['state']=='queued' and time.monotonic()<deadline: time.sleep(.01)
|
||||
self.assertEqual(self.manager.snapshot(motion)['state'],'running')
|
||||
self.manager.set_phase(self.manager._events[motion],'收尾')
|
||||
with self.assertRaisesRegex(RuntimeError,'机械臂 89.*对孔.*收尾'):
|
||||
self.manager.submit('reset_home',{'robot-control'},lambda e:{'ok':True})
|
||||
self.manager.cancel_resources({'robot-control'})
|
||||
self.assertTrue(self.manager._events[motion].is_set())
|
||||
self.assertFalse(self.manager._events[camera].is_set())
|
||||
finally: gate.set()
|
||||
|
||||
def test_cancelled_task_cannot_report_late_success_or_resume(self):
|
||||
started=threading.Event(); release=threading.Event(); calls=[]
|
||||
def old_task(event):
|
||||
calls.append('old'); started.set(); release.wait(2)
|
||||
return {'ok':True}
|
||||
token=self.manager.submit('stage_track',{'robot-control'},old_task)
|
||||
self.assertTrue(started.wait(1))
|
||||
self.manager.cancel_resources({'robot-control'}); release.set()
|
||||
result=self.wait(token)
|
||||
self.assertEqual(result['state'],'cancelled'); self.assertFalse(result['result']['ok'])
|
||||
fresh=self.manager.submit('real_up',{'robot-control'},lambda e:{'ok':True})
|
||||
self.assertEqual(self.wait(fresh)['state'],'succeeded')
|
||||
self.assertEqual(calls,['old'])
|
||||
|
||||
Reference in New Issue
Block a user