Initial import of ORISYS arm workstation
This commit is contained in:
cheng
@@ -0,0 +1,184 @@
|
||||
"""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'}
|
||||
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()
|
||||
|
||||
|
||||
# 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:
|
||||
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'])
|
||||
# Only the next explicit 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)
|
||||
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
|
||||
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()
|
||||
Reference in New Issue
Block a user