cheng
269 lines
14 KiB
Python
269 lines
14 KiB
Python
"""Isolated DDS-domain integration fixture. All hardware transports are fake."""
|
|
import threading
|
|
import time
|
|
from pathlib import Path
|
|
|
|
# The caller assigns a private ROS_DOMAIN_ID before importing rclpy.
|
|
import rclpy
|
|
from rclpy.executors import MultiThreadedExecutor
|
|
from rclpy.node import Node
|
|
from arm_msgs.srv import GripperExecute
|
|
from arm_application.dh_pgea import GripperState
|
|
import arm_application.dh_pgea_node as driver
|
|
|
|
|
|
class FakeGripper:
|
|
writes=[]
|
|
def __init__(self,**kwargs):
|
|
self.state=GripperState(init_state=1,grip_state=1,position=300,target_position=300,target_force=33)
|
|
def open(self): pass
|
|
def close(self): pass
|
|
def probe(self): return 1
|
|
def get_run_states(self,**kwargs): return self.state
|
|
def initialize(self): self.writes.append('INIT')
|
|
def move(self,pos,force,speed):
|
|
self.writes.append((pos,force,speed));self.state.position=pos;self.state.target_position=pos;return True
|
|
|
|
|
|
def main():
|
|
driver.DhPgeaGripper=FakeGripper
|
|
rclpy.init(args=['--ros-args','-r','__ns:=/station/grippers/arm_89','-p','auto_init:=false'])
|
|
device=driver.DhPgeaNode()
|
|
assert device.get_namespace()=='/station/grippers/arm_89',device.get_namespace()
|
|
client_node=Node('station_test_client')
|
|
executor=MultiThreadedExecutor(num_threads=2);executor.add_node(device);executor.add_node(client_node)
|
|
thread=threading.Thread(target=executor.spin,daemon=True);thread.start()
|
|
try:
|
|
client=client_node.create_client(GripperExecute,'/station/grippers/arm_89/execute')
|
|
assert client.wait_for_service(timeout_sec=3)
|
|
def command(name,position=0,force=33,speed=25):
|
|
req=GripperExecute.Request();req.command.command=name;req.command.position=float(position)
|
|
req.command.force=float(force);req.command.speed=float(speed)
|
|
future=client.call_async(req);deadline=time.monotonic()+3
|
|
while not future.done() and time.monotonic()<deadline: time.sleep(.01)
|
|
assert future.done(),'service did not respond'
|
|
return future.result()
|
|
assert command('connect').success
|
|
assert FakeGripper.writes==[],FakeGripper.writes
|
|
assert not command('move',1,force=999).success
|
|
assert not command('move',float('nan')).success
|
|
assert not command('rotate_to',90).success
|
|
assert FakeGripper.writes==[]
|
|
assert command('move',10).success
|
|
assert FakeGripper.writes==[(500,33,25)],FakeGripper.writes
|
|
assert command('connect').status.finger_position_per_mille==500.
|
|
print('ROS typed service, namespace, bounds and no-auto-init PASS',flush=True)
|
|
finally:
|
|
executor.shutdown(timeout_sec=3);thread.join(timeout=3)
|
|
device.destroy_node();client_node.destroy_node();rclpy.shutdown()
|
|
# Camera subscriptions must follow consumers, not all configured cameras.
|
|
from station import operator_camera_view as camera
|
|
cams=[{'id':'left','role':'wrist'},{'id':'right','role':'wrist'},{'id':'sim_wrist','role':'wrist'}]
|
|
camera.watch(set(),set());node=camera._open_node(cams)
|
|
try:
|
|
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)
|
|
message=Image();message.width=4;message.height=4;message.step=12;message.encoding='rgb8';message.data=bytes([90,20,10])*16
|
|
deadline=time.monotonic()+3
|
|
while 'left_color' not in camera.previews() and time.monotonic()<deadline:
|
|
publisher.publish(message);rclpy.spin_once(node,timeout_sec=.05);camera.publish_latest()
|
|
blob,width,height=camera.previews()['left_color']
|
|
assert (width,height)==(4,4) and blob[:3]==bytes([90,20,10])
|
|
import tempfile
|
|
from station.cam_bridge import _export
|
|
shms={}
|
|
try:
|
|
with tempfile.TemporaryDirectory() as directory:
|
|
_export(Path(directory),shms,{})
|
|
assert shms
|
|
first=next(iter(shms.values()))[1]
|
|
assert camera._shm_read(first)[3]==blob
|
|
finally:
|
|
for _,shm in shms.values(): shm.close();shm.unlink()
|
|
camera.watch({'right_depth'},set());node.sync_streams()
|
|
assert set(node._streams.items)=={'right_depth'}
|
|
camera.watch(set(),{'left_color'});node.sync_streams()
|
|
assert set(node._streams.items)=={'left_color'}
|
|
camera.watch(set(),set());node.sync_streams()
|
|
assert not node._streams.items and not node._detections.items
|
|
print('ROS dynamic camera subscriptions PASS',flush=True)
|
|
finally:
|
|
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
|
|
from sensor_msgs.msg import JointState
|
|
from controller_manager_msgs.srv import ListControllers, SwitchController
|
|
from controller_manager_msgs.msg import ControllerState
|
|
from rclpy.action import ActionServer, CancelResponse
|
|
from rclpy.callback_groups import ReentrantCallbackGroup
|
|
from control_msgs.action import FollowJointTrajectory
|
|
from station.tasks import Cancelled
|
|
gateway=RosGateway(load_site_dict())
|
|
fake=Node('fake_controller')
|
|
controller_state = ['active']
|
|
switches = []
|
|
def controllers(req,resp):
|
|
state=ControllerState();state.name='rizon_arm_controller';state.state=controller_state[0]
|
|
state.claimed_interfaces=[f"{arm['sn']}_joint{i}/position" for i in range(1,8)] if state.state=='active' else []
|
|
resp.controller=[state];return resp
|
|
fake.create_service(ListControllers,'/controller_manager/list_controllers',controllers)
|
|
def switch(req,resp):
|
|
switches.append((list(req.activate_controllers),list(req.deactivate_controllers)))
|
|
controller_state[0]='inactive' if req.deactivate_controllers else 'active'
|
|
resp.ok=True;return resp
|
|
fake.create_service(SwitchController,'/controller_manager/switch_controller',switch)
|
|
group=ReentrantCallbackGroup()
|
|
def execute(handle):
|
|
end=time.monotonic()+.7
|
|
while time.monotonic()<end:
|
|
if handle.is_cancel_requested:
|
|
handle.canceled();return FollowJointTrajectory.Result()
|
|
time.sleep(.01)
|
|
handle.succeed();return FollowJointTrajectory.Result()
|
|
action=ActionServer(fake,FollowJointTrajectory,'/rizon_arm_controller/follow_joint_trajectory',
|
|
execute_callback=execute,cancel_callback=lambda request:CancelResponse.ACCEPT,callback_group=group)
|
|
gateway.executor.add_node(fake)
|
|
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)
|
|
cancel=threading.Event();timer=threading.Timer(.3,cancel.set);timer.start()
|
|
try:
|
|
gateway.move_joints(arm,[.01]*7,.15,cancel)
|
|
raise AssertionError('expected action cancellation')
|
|
except Cancelled: pass
|
|
timer.join()
|
|
assert not gateway._goals
|
|
joint_stop=threading.Event()
|
|
def joint_feedback():
|
|
while not joint_stop.wait(.03): gateway._joint(msg)
|
|
joint_thread=threading.Thread(target=joint_feedback);joint_thread.start()
|
|
try:
|
|
assert gateway.stop_arm(arm)['ok']
|
|
assert gateway.confirm_stopped(arm)['ok']
|
|
assert controller_state[0]=='inactive'
|
|
assert switches[-1]==([],['rizon_arm_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'],[])
|
|
wrong={**arm,'sn':'OtherRobot'}
|
|
try:
|
|
gateway.stop_arm(wrong)
|
|
raise AssertionError('wrong-arm stop accepted')
|
|
except RuntimeError as exc: assert '不属于' in str(exc)
|
|
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()
|
|
gateway.watch_tactile(False)
|
|
assert not gateway.tactile_frames() and not gateway._tactile_images.items
|
|
print('ROS tactile driver reuse, unchanged force values and preview lifetime PASS',flush=True)
|
|
print('ROS persistent gateway, arm identity and trajectory cancellation PASS',flush=True)
|
|
finally:
|
|
action.destroy();gateway.executor.remove_node(fake);fake.destroy_node();gateway.close()
|
|
|
|
|
|
if __name__=='__main__': main()
|