def __init__(self):
#Instances
self.gripper = Gripper()
self.m = SendMove()
self.t = tf.TransformListener()
self.armPose = ""
#Subscribers
rospy.Subscriber("/tf", TFMessage, self.getToolTF)
rospy.Subscriber("tool_velocity", TwistStamped, self.getToolVelocity)
#Open gripper
self.gripper.moveGripper("Open")
#Go to initial position
self.m.sendMove(self.m.buildMove('j', '', self.m.getPos('test3')))
self.waitForArm()
def __init__(self, context):
self.context = context
self.flag = True
self.needToRetract = False
self.draw_back_guidewire_curcuit_flag = True
self.guidewireProgressMotor = OrientalMotor(20, 21, True)
self.guidewireRotateMotor = MaxonMotor(2, "EPOS2", "MAXON SERIAL V2",
"USB", "USB0", 1000000)
self.catheterMotor = OrientalMotor(14, 15, True)
self.angioMotor = OrientalMotor(23, 24, False)
self.gripperFront = Gripper(7)
self.gripperBack = Gripper(8)
self.infraredReflectiveSensor = InfraredReflectiveSensor()
self.dispatchTask = threading.Thread(None, self.listening)
#self.dispatchTask.start()
self.cptt = 0
self.global_state = 0
number = int(message[2:-1])
print("Command: " + mess + " Number: " + str(number))
if __name__ == '__main__':
mes = "Q"
num = 0
# Makes sure the script is run as sudo.
if not os.geteuid() == 0:
sys.exit("\nMust be root to run this script!\n")
# Set up the USB Connection to the Arduino
usb_options = ['/dev/ttyUSB0', '/dev/ttyUSB1', '/dev/ttyUSB2']
usb_options = filter(os.path.exists, usb_options)
assert (len(usb_options) == 1) # Exist if no usb connection.
usb_port = usb_options[0]
# Open the communication thread.
print("Communication Thread Started.")
communication_motor = tcp_server_motor(usb_port)
communication_motor.start()
# Set up and start gripper thread. /--- EXPERIMENTAL ---/
print("Gripper Thread Started.")
pheeno_gripper = Gripper()
communication_gripper = threading.Thread(target=myGripper.gripperMove,
args=(servo, angle))
communication_gripper.start()
class Dispatcher(object):
def __init__(self, context):
self.context = context
self.flag = True
self.needToRetract = False
self.draw_back_guidewire_curcuit_flag = True
self.guidewireProgressMotor = OrientalMotor(20, 21, True)
self.guidewireRotateMotor = MaxonMotor(2, "EPOS2", "MAXON SERIAL V2",
"USB", "USB0", 1000000)
self.catheterMotor = OrientalMotor(14, 15, True)
self.angioMotor = OrientalMotor(23, 24, False)
self.gripperFront = Gripper(7)
self.gripperBack = Gripper(8)
self.infraredReflectiveSensor = InfraredReflectiveSensor()
self.dispatchTask = threading.Thread(None, self.listening)
#self.dispatchTask.start()
self.cptt = 0
self.global_state = 0
def set_global_state(self, state):
self.global_state = state
def listening(self):
while self.flag:
if not self.context.get_system_status():
self.guidewireRotateMotor.close_device()
self.guidewireProgressMotor.close_device()
self.catheterMotor.close_device()
self.angioMotor.close_device()
sys.exit()
self.flag = False
print "system terminated"
else:
self.decode()
time.sleep(0.05)
def decode(self):
if self.context.get_catheter_move_instruction_sequence_length() > 0:
msg = self.context.fetch_latest_catheter_move_msg()
if self.draw_back_guidewire_curcuit_flag == False:
return
if msg.get_motor_orientation() == 0:
self.catheterMotor.set_speed(msg.get_motor_speed())
return
elif msg.get_motor_orientation() == 1:
self.catheterMotor.set_speed(-msg.get_motor_speed())
return
if not self.needToRetract:
if self.context.get_guidewire_progress_instruction_sequence_length(
) > 0:
self.set_global_state(
self.infraredReflectiveSensor.read_current_state())
if self.global_state == 0:
msg = self.context.fetch_latest_guidewire_progress_move_msg(
)
if self.draw_back_guidewire_curcuit_flag == False:
return
if msg.get_motor_orientation() == 0 and abs(
msg.get_motor_speed()) < 40 * 2 * 60:
#print -msg.get_motor_speed()
self.guidewireProgressMotor.set_speed(
-msg.get_motor_speed())
self.cptt = 0
elif msg.get_motor_orientation() == 1 and abs(
msg.get_motor_speed()) < 40 * 2 * 60:
#print msg.get_motor_speed()
self.guidewireProgressMotor.set_speed(
msg.get_motor_speed())
else:
self.guidewireProgressMotor.set_speed(0)
elif self.global_state == 2:
#print "retract"
self.guidewireProgressMotor.set_speed(0)
self.needToRetract = True
retractTask = threading.Thread(
None, self.draw_back_guidewire_curcuit)
retractTask.start()
elif self.global_state == 1:
#print "hehe", self.global_guidewire_distance
self.guidewireProgressMotor.set_speed(100)
elif self.global_state == 3:
self.guidewireProgressMotor.set_speed(0)
if self.context.get_guidewire_rotate_instruction_sequence_length(
) > 0:
msg = self.context.fetch_latest_guidewire_rotate_move_msg()
speed = msg.get_motor_speed()
position = (msg.get_motor_position() * 4000) / 360
if self.draw_back_guidewire_curcuit_flag == False:
return
if msg.get_motor_orientation() == 0:
self.guidewireRotateMotor.rm_move(speed)
pass
elif msg.get_motor_orientation() == 1:
self.guidewireRotateMotor.rm_move(-speed)
pass
if self.context.get_contrast_media_push_instruction_sequence_length(
) > 0:
msg = self.context.fetch_latest_contrast_media_push_move_msg()
ret = msg.get_motor_speed()
if self.draw_back_guidewire_curcuit_flag == False:
return
if msg.get_motor_orientation() == 0:
self.angioMotor.set_speed(-ret)
elif msg.get_motor_orientation() == 1:
self.angioMotor.set_speed(ret)
if self.context.get_retract_instruction_sequence_length() > 0:
if self.draw_back_guidewire_curcuit_flag == False:
return
self.draw_back_guidewire_curcuit()
if self.context.get_injection_command_sequence_length() > 0:
msg = self.context.fetch_latest_injection_msg_msg()
#print "injection command", msg.get_speed(),msg.get_volume()
if msg.get_volume() < 18:
self.angioMotor.set_pos_speed(msg.get_speed())
self.angioMotor.set_position(msg.get_volume())
self.angioMotor.push_contrast_media()
elif msg.get_volume() == 500.0:
self.angioMotor.pull_back()
def push_guidewire_back(self):
#self.context.clear()
self.draw_back_guidewire_curcuit_flag == False
#self.gripperFront.gripper_chuck_loosen()
#self.gripperBack.gripper_chuck_loosen()
#time.sleep(1)
# fasten front gripper
self.gripperFront.gripper_chuck_fasten()
# self-tightening chunck
self.gripperBack.gripper_chuck_fasten()
time.sleep(1)
self.guidewireRotateMotor.rm_move_to_position(90, 4000) # +/loosen
time.sleep(3)
#self.gripperFront.gripper_chuck_loosen()
#time.sleep(1)
self.guidewireProgressMotor.set_speed(-800)
self.global_state = self.infraredReflectiveSensor.read_current_state()
while self.global_state != 1:
time.sleep(0.5)
self.global_state = self.infraredReflectiveSensor.read_current_state(
)
print "retracting", self.global_state
print "back limitation arrived"
self.guidewireProgressMotor.set_speed(0)
self.guidewireRotateMotor.rm_move_to_position(90, -4000)
time.sleep(3)
self.gripperFront.gripper_chuck_loosen()
self.gripperBack.gripper_chuck_loosen()
self.draw_back_guidewire_curcuit_flag == True
self.needToRetract = False
def push_guidewire(self):
# self.context.clear()
self.draw_back_guidewire_curcuit_flag == False
for i in range(0, 2):
self.gripperFront.gripper_chuck_loosen()
self.gripperBack.gripper_chuck_loosen()
time.sleep(1)
self.gripperFront.gripper_chuck_fasten()
self.gripperBack.gripper_chuck_fasten()
time.sleep(1)
self.guidewireRotateMotor.rm_move_to_position(40, 5000) # +/loosen
time.sleep(8)
self.gripperFront.gripper_chuck_loosen()
self.global_guidewire_distance = self.ultraSoundModule.read_current_distance(
)
self.guidewireProgressMotor.set_speed(600)
while self.global_guidewire_distance > (self.frontLimitDistance +
2):
time.sleep(0.5)
self.global_guidewire_distance = self.ultraSoundModule.read_current_distance(
)
print "pushing"
print "pushing limitation arrived"
#time.sleep(15)
self.guidewireProgressMotor.set_speed(0)
time.sleep(1)
self.gripperFront.gripper_chuck_loosen()
self.gripperBack.gripper_chuck_loosen()
time.sleep(1)
self.gripperFront.gripper_chuck_fasten()
self.gripperBack.gripper_chuck_fasten()
time.sleep(1)
self.guidewireRotateMotor.rm_move_to_position(40, -8000)
time.sleep(8)
self.gripperFront.gripper_chuck_loosen()
#time.sleep(1)
self.guidewireProgressMotor.set_speed(-600)
while self.global_guidewire_distance < (self.behindLimitDistance -
2):
time.sleep(0.5)
self.global_guidewire_distance = self.ultraSoundModule.read_current_distance(
)
print "fetching"
print "fetch limitation arrived"
#self.guidewireProgressMotor.set_speed(-400)
#time.sleep(15)
self.guidewireProgressMotor.set_speed(0)
time.sleep(1)
self.draw_back_guidewire_curcuit_flag == True
def push_guidewire_advance(self):
self.guidewireProgressMotor.set_speed(800)
self.global_state = self.infraredReflectiveSensor.read_current_state()
while self.global_state != 2:
time.sleep(0.5)
self.global_state = self.infraredReflectiveSensor.read_current_state(
)
#print "pushing", self.global_state
#print "front limitation arrived"
self.guidewireProgressMotor.set_speed(0)
#self.guidewireRotateMotor.rm_move_to_position(90, -8000)
#time.sleep(4)
def multitime_push_guidewire(self):
for i in range(0, 8):
dispatcher.push_guidewire_advance()
dispatcher.push_guidewire_back()
print(i)
def draw_guidewire_back(self):
#self.draw_back_guidewire_curcuit_flag == False
#self.gripperFront.gripper_chuck_loosen()
#self.gripperBack.gripper_chuck_fasten()
#time.sleep(1)
#self.guidewireRotateMotor.rm_move_to_position(85,4000)
#time.sleep(3)
self.guidewireProgressMotor.set_speed(-800)
self.global_state = self.infraredReflectiveSensor.read_current_state()
while self.global_state != 1:
time.sleep(0.5)
self.global_state = self.infraredReflectiveSensor.read_current_state(
)
#print "retracting", self.global_state
#print "back limitation arrived"
self.guidewireProgressMotor.set_speed(0)
#self.guidewireRotateMotor.rm_move_to_position(85, -4000)
#time.sleep(4)
#self.draw_back_guidewire_curcuit_flag == True
#self.needToRetract = False
def draw_guidewire_advance(self):
self.gripperFront.gripper_chuck_loosen()
self.gripperBack.gripper_chuck_loosen()
time.sleep(1)
self.gripperFront.gripper_chuck_fasten()
self.gripperBack.gripper_chuck_fasten()
time.sleep(1)
self.guidewireRotateMotor.rm_move_to_position(80, 4000)
time.sleep(3)
self.guidewireProgressMotor.set_speed(800)
self.global_state = self.infraredReflectiveSensor.read_current_state()
while self.global_state != 2:
time.sleep(0.5)
self.global_state = self.infraredReflectiveSensor.read_current_state(
)
#print "advancing", self.global_state
#print "front limitation arrived"
self.guidewireProgressMotor.set_speed(0)
#self.gripperFront.gripper_chuck_fasten()
#self.gripperBack.gripper_chuck_fasten()
time.sleep(1)
self.guidewireRotateMotor.rm_move_to_position(80, -4000)
time.sleep(3)
# self.gripperFront.gripper_chuck_fasten()
# time.sleep(1)
#self.gripperFront.gripper_chuck_loosen()
self.gripperBack.gripper_chuck_loosen()
time.sleep(1)
def multitime_draw_back_guidewire(self):
for i in range(0, 8):
dispatcher.draw_guidewire_advance()
dispatcher.draw_guidewire_back()
print(i)
def automatic_procedure(self):
self.angioMotor.set_pos_speed(2)
self.angioMotor.set_position(5)
self.angioMotor.push_contrast_media()
print "angiographing finish"
time.sleep(5)
self.multitime_push_guidewire()
self.multitime_draw_back_guidewire()
def push_and_pull(self):
self.multitime_push_guidewire()
self.multitime_draw_back_guidewire()
def loosen(self):
self.gripperBack.gripper_chuck_fasten()
time.sleep(1)
self.gripperBack.gripper_chuck_loosen()
#self.gripperBack.gripper_chuck_loosen()
time.sleep(1)
#!/usr/bin/env micropython from DriveTrain import DriveTrain from Gripper import Gripper import time driveTrain = DriveTrain() gripper = Gripper() gripper.lowerMotor(30) driveTrain.driveToLine(-20, 8, ["Green", "Blue"], ["Black", "Brown"]) driveTrain.driveForward(-40, 13) driveTrain.turnAngle(-20, 90) driveTrain.followLine(-20, 9, ["Black", "Brown"], 10) driveTrain.driveForward(-40, -40) driveTrain.driveForward(-20, -5) time.sleep(3) driveTrain.followLine(-20, 10, ["Black", "Brown"], 40) driveTrain.turnAngle(20, 180) driveTrain.driveToLine(-20, 10, ["Black", "Brown"],["Black", "Brown"]) gripper.lowerMotor(-30) driveTrain.driveForward(20, 10)
class UR():
def __init__(self):
#Instances
self.gripper = Gripper()
self.m = SendMove()
self.t = tf.TransformListener()
self.armPose = ""
#Subscribers
rospy.Subscriber("/tf", TFMessage, self.getToolTF)
rospy.Subscriber("tool_velocity", TwistStamped, self.getToolVelocity)
#Open gripper
self.gripper.moveGripper("Open")
#Go to initial position
self.m.sendMove(self.m.buildMove('j', '', self.m.getPos('test3')))
self.waitForArm()
def onTF(self, link, state):
"""[This function is used to do pick and place based on TF positions]
Arguments:
link {String} -- [This is the TF name]
state {String} -- ["Pick" or "Place"]
"""
transform = self.t.lookupTransform("ur3/base", link, rospy.Time(0))
rospy.loginfo_throttle(5, "Tranformation found for %s" % link)
x = transform[0][0]
y = transform[0][1]
z = transform[0][2]
qrx = transform[1][0]
qry = transform[1][1]
qrz = transform[1][2]
qrw = transform[1][3]
[roll, pitch,
yaw] = tf.transformations.euler_from_quaternion([qrx, qry, qrz, qrw])
[rx, ry, rz] = self.m.euler2Rot(roll, pitch, yaw)
blend_radius = 0.01
#self.m.sendMove(self.m.buildMove('j', 'p', [x, y, z + 0.05, rx, ry, rz], blend_radius))
#self.waitForArmBlend([x, y, (z + 0.05), rx, ry, rz], blend_radius+0.02)
#self.moveTool([x, y, z, rx, ry, rz])
self.m.sendMove(
self.m.buildBlendMove("l", "p", [x, y, z + 0.05, rx, ry, rz],
[x, y, z, rx, ry, rz], blend_radius))
self.waitForArmBlend([x, y, z + 0.05, rx, ry, rz], blend_radius)
if state == "Pick":
self.gripper.moveGripper("Close")
elif state == "Place":
self.gripper.moveGripper("Open")
#self.moveTool([x, y, z + 0.05, rx, ry, rz])
#----------Code for object following----------
# pose = [x, y, z + 0.05 * sin(2.3*rospy.get_time()) + 0.05, rx, ry, rz]
# array = list(pose)
# a = 1.0
# v = 1.0
# t = 0.05
# st = "servoj(get_inverse_kin(p%s), a=%s, v=%s, t=%s)"%(array, a, v, t)
# self.m.sendMove(st)
# time.sleep(t)
def moveTool(self, pose):
"""[This function is used to send the tool to a position in linear tool space]
Arguments:
pose {List} -- [x,y,z,rx,ry,rz]
"""
if self.isPoseReachable(pose):
self.m.sendMove(self.m.buildMove('l', 'p', pose))
self.waitForArm()
def goHolder(self, holderPlace, picking):
"""[This function is used to do pick and place task to/from a holder place on Suii]
Arguments:
holderPlace {String} -- ["HOLDER_1", "HOLDER_2", "HOLDER_3"]
picking {String} -- ["Pick" or "Place"]
"""
rospy.loginfo("Going to holderplace %s", holderPlace)
if holderPlace == "HOLDER_1":
self.m.sendMove(
self.m.buildMove('j', '', self.m.getPos('finalPreHolder1')))
self.waitForArm()
self.m.sendMove(
self.m.buildMove('j', '', self.m.getPos('finalHolder1')))
self.waitForArm()
if picking == "Pick":
self.gripper.moveGripper("Close")
elif picking == "Place":
self.gripper.moveGripper("Open")
self.m.sendMove(
self.m.buildMove('j', '', self.m.getPos('finalPreHolder1')))
self.waitForArm()
elif holderPlace == "HOLDER_2":
self.m.sendMove(
self.m.buildMove('j', '', self.m.getPos('finalPreHolder2')))
self.waitForArm()
self.m.sendMove(
self.m.buildMove('j', '', self.m.getPos('finalHolder2')))
self.waitForArm()
if picking == "Pick":
self.gripper.moveGripper("Close")
elif picking == "Place":
self.gripper.moveGripper("Open")
self.m.sendMove(
self.m.buildMove('j', '', self.m.getPos('finalPreHolder2')))
self.waitForArm()
elif holderPlace == "HOLDER_3":
self.m.sendMove(
self.m.buildMove('j', '', self.m.getPos('finalPreHolder3')))
self.waitForArm()
self.m.sendMove(
self.m.buildMove('j', '', self.m.getPos('finalHolder4')))
self.waitForArm()
if picking == "Pick":
self.gripper.moveGripper("Close")
elif picking == "Place":
self.gripper.moveGripper("Open")
self.m.sendMove(
self.m.buildMove('j', '', self.m.getPos('finalPreHolder2')))
self.waitForArm()
def waitForArm(self):
"""[This function waits till the arm reached it's position, by waiting till tool velocity is zero.]
"""
time.sleep(
0.3
) #0.3 value for real ur3 otherwise arm has no time to get speed
while (abs(self.xVel) > 0.0001 or abs(self.yVel) > 0.0001
or abs(self.zVel) > 0.0001) and not rospy.is_shutdown():
rospy.loginfo_throttle(1, "Robot is moving to position")
rospy.loginfo("Robot reached position")
def waitForArmBlend(self, pose, radius):
while abs(
sqrt((self.tool[0] - pose[0])**2 +
(self.tool[1] - pose[1])**2 +
(self.tool[2] - pose[2])**2)) > radius:
rospy.loginfo_throttle(1, "Moving to blend radius")
rospy.loginfo("Robot reached blend radius")
def isPoseReachable(self, pose):
"""[This function checks if the goal position is within reach of UR3]
Arguments:
pose {list} -- [x,y,z,rx,ry,rz]
Returns:
[bool] -- [True if is within reach, else false]
"""
maxReach = 0.538
minReach = 0.064
x = pose[0]
y = pose[1]
z = pose[2]
distance = sqrt(x**2 + y**2 + z**2)
if distance < maxReach and distance > minReach:
rospy.loginfo("Positon in reach. Distance = %s", distance)
return True
else:
rospy.logwarn("Out of reach, command not sended. Distance = %s",
distance)
def getToolVelocity(self, msg):
self.xVel = msg.twist.linear.x
self.yVel = msg.twist.linear.y
self.zVel = msg.twist.linear.z
self.rxVel = msg.twist.angular.x
self.ryVel = msg.twist.angular.y
self.rzVel = msg.twist.angular.z
def getToolTF(self, msg):
# if len(msg.transforms) == 0:
# return
if msg.transforms[-1].child_frame_id == "ur3/tool0_controller":
x = msg.transforms[-1].transform.translation.x
y = msg.transforms[-1].transform.translation.y
z = msg.transforms[-1].transform.translation.z
qx = msg.transforms[-1].transform.rotation.x
qy = msg.transforms[-1].transform.rotation.y
qz = msg.transforms[-1].transform.rotation.z
qw = msg.transforms[-1].transform.rotation.w
[roll, pitch,
yaw] = tf.transformations.euler_from_quaternion([qx, qy, qz, qw])
[rx, ry, rz] = self.m.euler2Rot(roll, pitch, yaw)
self.tool = [x, y, z, rx, ry, rz]
def __init__(self, context, local_mode=0):
self.context = context
# ---------------------------------------------------------------------------------------------
# initialisation
# ---------------------------------------------------------------------------------------------
self.flag = True
self.cptt = 0
self.global_state = 0
self.needToRetract = False
self.draw_back_guidewire_curcuit_flag = True
self.number_of_cycles = 0
# ---------------------------------------------------------------------------------------------
# execution units of the interventional robot
# ---------------------------------------------------------------------------------------------
self.guidewireProgressMotor = OrientalMotor(20, 21, True)
self.guidewireRotateMotor = OrientalMotor(19, 26, True)
self.catheterMotor = OrientalMotor(17, 27, True)
self.angioMotor = OrientalMotor(23, 24, False)
self.gripperFront = Gripper(7)
self.gripperBack = Gripper(8)
# ---------------------------------------------------------------------------------------------
# sensors
# ---------------------------------------------------------------------------------------------
self.infraredReflectiveSensor = InfraredReflectiveSensor()
# ---------------------------
# feedback
# ---------------------------
self.forcefeedback = Feedback("/dev/ttyUSB1", 9600, 8, 'N', 1)
self.torquefeedback = Feedback("/dev/ttyUSB0", 9600, 8, 'N', 1)
# ---------------------------------------------------------------------------------------------
# EmergencySwitch
# ---------------------------------------------------------------------------------------------
self.switch = EmergencySwitch()
self.emSwitch = 1
self.lastSwitch = 0
self.em_count = 0
# ---------------------------------------------------------------------------------------------
# speed parameters
# ---------------------------------------------------------------------------------------------
self.speedProgress = 1000
self.speedRotate = 60
self.speedCatheter = 10
self.rotateTime = 180 / self.speedRotate
self.pos_speed = 5
self.position_cgf = 2
self.position_cgb = -100
# ---------------------------------------------------------------------------------------------
# real time task to parse commandes in context
# ---------------------------------------------------------------------------------------------
self.dispatchTask = threading.Thread(
None, self.do_parse_commandes_in_context)
self.dispatchTask.start()
self.aquirefeedbackTask = threading.Thread(None,
self.aquirefeedback_context)
self.aquirefeedbackTask.start()
import time
import serial
import socket
import os.path
import sys
from Gripper import Gripper
myGripper = Gripper()
PORT = '12345'
usbOptions = ['/dev/ttyUSB0', '/dev/ttyUSB1', '/dev/ttyUSB2']
usbOptions = filter(os.path.exists, usbOptions)
assert(len(usbOptions) == 1)
usbPort = usbOptions[0]
s = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
s.bind(('0.0.0.0', 12345))
s.listen(1)
client, _ = s.accept()
while True:
myGripper.gripperMove(3, 100, 0.1)
try:
usb = serial.Serial(usbPort, 9600, timeout=10)
usb.open()
time.sleep(2) # give the serial time to get opened
except serial.SerialException:
print("Could not connect to requested port")
time.sleep(1)
continue
while True:
class Dispatcher(object):
"""
description:this class plays an role in th command and control of the interventional robot which includes:
-- the control of GPIOs of the raspberryPi which connet motors, sensors and grippers
-- the distribution of tasks in different threads
-- the command and control of the actions of interventional robot in surgery
author:Cheng WANG
"""
def __init__(self, context, local_mode=0):
self.context = context
# ---------------------------------------------------------------------------------------------
# initialisation
# ---------------------------------------------------------------------------------------------
self.flag = True
self.cptt = 0
self.global_state = 0
self.needToRetract = False
self.draw_back_guidewire_curcuit_flag = True
self.number_of_cycles = 0
# ---------------------------------------------------------------------------------------------
# execution units of the interventional robot
# ---------------------------------------------------------------------------------------------
self.guidewireProgressMotor = OrientalMotor(20, 21, True)
self.guidewireRotateMotor = OrientalMotor(19, 26, True)
self.catheterMotor = OrientalMotor(17, 27, True)
self.angioMotor = OrientalMotor(23, 24, False)
self.gripperFront = Gripper(7)
self.gripperBack = Gripper(8)
# ---------------------------------------------------------------------------------------------
# sensors
# ---------------------------------------------------------------------------------------------
self.infraredReflectiveSensor = InfraredReflectiveSensor()
# ---------------------------
# feedback
# ---------------------------
self.forcefeedback = Feedback("/dev/ttyUSB1", 9600, 8, 'N', 1)
self.torquefeedback = Feedback("/dev/ttyUSB0", 9600, 8, 'N', 1)
# ---------------------------------------------------------------------------------------------
# EmergencySwitch
# ---------------------------------------------------------------------------------------------
self.switch = EmergencySwitch()
self.emSwitch = 1
self.lastSwitch = 0
self.em_count = 0
# ---------------------------------------------------------------------------------------------
# speed parameters
# ---------------------------------------------------------------------------------------------
self.speedProgress = 1000
self.speedRotate = 60
self.speedCatheter = 10
self.rotateTime = 180 / self.speedRotate
self.pos_speed = 5
self.position_cgf = 2
self.position_cgb = -100
# ---------------------------------------------------------------------------------------------
# real time task to parse commandes in context
# ---------------------------------------------------------------------------------------------
self.dispatchTask = threading.Thread(
None, self.do_parse_commandes_in_context)
self.dispatchTask.start()
self.aquirefeedbackTask = threading.Thread(None,
self.aquirefeedback_context)
self.aquirefeedbackTask.start()
def set_global_state(self, state):
self.global_state = state
def do_parse_commandes_in_context(self):
"""
determine system's status and start to decode or to close devices
"""
while self.flag:
if not self.context.get_system_status():
self.guidewireRotateMotor.close_device()
self.guidewireProgressMotor.close_device()
self.catheterMotor.close_device()
self.angioMotor.close_device()
sys.exit()
self.flag = False
print "system terminated"
else:
self.emSwitch = self.switch.read_current_state()
if self.emSwitch == 1:
time.sleep(0.02)
self.guidewireRotateMotor.standby()
self.guidewireProgressMotor.standby()
self.catheterMotor.standby()
self.angioMotor.standby()
self.lastSwitch = 1
elif self.emSwitch == 0 and self.lastSwitch == 1:
self.guidewireRotateMotor.enable()
self.guidewireProgressMotor.enable()
self.catheterMotor.enable()
self.angioMotor.enable()
self.lastSwitch = 0
self.decode()
elif self.emSwitch == 0 and self.lastSwitch == 0:
self.decode()
time.sleep(0.05)
def decode(self):
"""
decode messages in the sequence and performe operations
"""
# ------------------------------------------------------------------------------
# advance according the set distance
# -----------------------------------------------------------------------------
if self.context.get_catheter_guidewire_push_sequence_lenght() > 0:
msg = self.context.fetch_latest_catheter_guidewire_push_msg()
if self.draw_back_guidewire_curcuit_flag == False:
return
speed = msg.get_motor_speed()
relative_position = msg.get_motor_relative_distance()
# ---------------------------------------------------------------------------------------------
# catheter execution case
# ---------------------------------------------------------------------------------------------
if self.context.get_catheter_move_instruction_sequence_length() > 0:
msg = self.context.fetch_latest_catheter_move_msg()
if self.draw_back_guidewire_curcuit_flag == False:
return
if msg.get_motor_orientation() == 0:
self.catheterMotor.set_speed(msg.get_motor_speed() / 10.0)
return
elif msg.get_motor_orientation() == 1:
self.catheterMotor.set_speed(-msg.get_motor_speed() / 10.0)
return
# ---------------------------------------------------------------------------------------------
# guidewire progress execution case
# ---------------------------------------------------------------------------------------------
if not self.needToRetract: # or not self.need ....
if self.context.get_guidewire_progress_instruction_sequence_length(
) > 0:
self.set_global_state(
self.infraredReflectiveSensor.read_current_state())
if self.global_state == 0:
msg = self.context.fetch_latest_guidewire_progress_move_msg(
)
if self.draw_back_guidewire_curcuit_flag == False:
return
if msg.get_motor_orientation() == 0 and abs(
msg.get_motor_speed()) < 40 * 2 * 60:
#print -msg.get_motor_speed()
self.guidewireProgressMotor.set_speed(
-msg.get_motor_speed())
self.cptt = 0
elif msg.get_motor_orientation() == 1 and abs(
msg.get_motor_speed()) < 40 * 2 * 60:
#print msg.get_motor_speed()
self.guidewireProgressMotor.set_speed(
msg.get_motor_speed())
else:
self.guidewireProgressMotor.set_speed(0)
elif self.global_state == 2:
#print "retract"
self.guidewireProgressMotor.set_speed(0)
self.needToRetract = True
retractTask = threading.Thread(None,
self.push_guidewire_back)
retractTask.start()
elif self.global_state == 1:
#print "hehe", self.global_guidewire_distance
self.guidewireProgressMotor.set_speed(self.speedProgress)
elif self.global_state == 3:
self.guidewireProgressMotor.set_speed(0)
# ---------------------------------------------------------------------------------------------
# guidewire rotate execution case
# ---------------------------------------------------------------------------------------------
if self.context.get_guidewire_rotate_instruction_sequence_length(
) > 0:
msg = self.context.fetch_latest_guidewire_rotate_move_msg()
speed = msg.get_motor_speed()
position = (msg.get_motor_position() * 4000) / 360
if self.draw_back_guidewire_curcuit_flag == False:
return
if msg.get_motor_orientation() == 0:
self.guidewireRotateMotor.set_speed(speed)
pass
elif msg.get_motor_orientation() == 1:
self.guidewireRotateMotor.set_speed(-speed)
pass
# ---------------------------------------------------------------------------------------------
# contrast media push execution case
# ---------------------------------------------------------------------------------------------
if self.context.get_contrast_media_push_instruction_sequence_length(
) > 0:
msg = self.context.fetch_latest_contrast_media_push_move_msg()
ret = msg.get_motor_speed()
if self.draw_back_guidewire_curcuit_flag == False:
return
if msg.get_motor_orientation() == 0:
self.angioMotor.set_speed(-ret)
elif msg.get_motor_orientation() == 1:
self.angioMotor.set_speed(ret)
if self.context.get_retract_instruction_sequence_length() > 0:
if self.draw_back_guidewire_curcuit_flag == False:
return
self.draw_back_guidewire_curcuit()
if self.context.get_injection_command_sequence_length() > 0:
msg = self.context.fetch_latest_injection_msg_msg()
#print "injection command", msg.get_speed(),msg.get_volume()
if msg.get_volume() < 0:
self.angioMotor.set_pos_speed(msg.get_speed())
self.angioMotor.set_position(msg.get_volume() / 4.5)
self.angioMotor.pull_contrast_media()
elif msg.get_volume() <= 30:
self.angioMotor.set_pos_speed(msg.get_speed())
self.angioMotor.set_position(msg.get_volume() / 4.5)
self.angioMotor.push_contrast_media()
def push_contrast_agent(self):
"""
Contrast agent push
"""
self.angioMotor.set_pos_speed(self.pos_speed)
self.angioMotor.set_position(self.position_cgf / 4.5)
self.angioMotor.push_contrast_media()
def pull_contrast_agent(self):
self.angioMotor.set_pos_speed(self.pos_speed)
self.angioMotor.set_position(self.position_cgb / 4.5)
self.angioMotor.pull_contrast_media()
def push_guidewire_back(self):
"""
the shifboard get back when guidewire progress
"""
self.context.clear_guidewire_message()
self.draw_back_guidewire_curcuit_flag == False
#self.gripperFront.gripper_chuck_loosen()
#self.gripperBack.gripper_chuck_loosen()
#time.sleep(1)
# fasten front gripper
self.gripperFront.gripper_chuck_fasten()
# self-tightening chunck
self.gripperBack.gripper_chuck_fasten()
time.sleep(1)
self.guidewireRotateMotor.set_speed(-self.speedRotate) # +/loosen
time.sleep(self.rotateTime)
self.guidewireRotateMotor.set_speed(0)
#self.gripperFront.gripper_chuck_loosen()
#time.sleep(1)
self.guidewireProgressMotor.set_speed(-self.speedProgress)
#time.sleep(3)
self.global_state = self.infraredReflectiveSensor.read_current_state()
while self.global_state != 1:
#self.global_state = self.infraredReflectiveSensor.read_current_state()
#if self.global_state == 4:
#self.global_state = self.infraredReflectiveSensor.read_current_state()
#continue
time.sleep(0.5)
self.global_state = self.infraredReflectiveSensor.read_current_state(
)
print "retracting", self.global_state
print "back limitation arrived"
self.guidewireProgressMotor.set_speed(0)
self.guidewireRotateMotor.set_speed(self.speedRotate)
time.sleep(self.rotateTime)
self.guidewireRotateMotor.set_speed(0)
self.gripperFront.gripper_chuck_loosen()
self.gripperBack.gripper_chuck_loosen()
self.draw_back_guidewire_curcuit_flag == True
self.context.clear_guidewire_message()
self.needToRetract = False
def push_guidewire_advance(self):
"""
the shiftboard advance with pushing guidewire
"""
#self.context.clear_guidewire_message()
self.guidewireProgressMotor.set_speed(self.speedProgress)
self.global_state = self.infraredReflectiveSensor.read_current_state()
while self.global_state != 2:
time.sleep(0.5)
self.global_state = self.infraredReflectiveSensor.read_current_state(
)
#print "pushing", self.global_state
#print "front limitation arrived"
self.guidewireProgressMotor.set_speed(0)
#self.guidewireRotateMotor.rm_move_to_position(90, -8000)
#time.sleep(4)
def multitime_push_guidewire(self):
"""
the process of pushing guidewire for several times
"""
self.define_number_of_cycles()
for i in range(0, self.number_of_cycles):
self.push_guidewire_advance()
self.push_guidewire_back()
print(i)
def draw_guidewire_back(self):
"""
the shiftboard go back with drawing back guidewire
"""
self.guidewireRotateMotor.set_speed(self.speedRotate)
time.sleep(self.rotateTime)
self.guidewireRotateMotor.set_speed(0)
self.gripperBack.gripper_chuck_loosen()
time.sleep(1)
self.guidewireProgressMotor.set_speed(-self.speedProgress)
self.global_state = self.infraredReflectiveSensor.read_current_state()
while self.global_state != 1:
time.sleep(0.5)
self.global_state = self.infraredReflectiveSensor.read_current_state(
)
#print "retracting", self.global_state
#print "back limitation arrived"
self.guidewireProgressMotor.set_speed(0)
#self.guidewireRotateMotor.rm_move_to_position(85, -4000)
#time.sleep(4)
#self.draw_back_guidewire_curcuit_flag == True
#self.needToRetract = False
def chuck_loosen(self):
self.gripperBack.gripper_chuck_fasten()
time.sleep(1)
self.guidewireRotateMotor.set_speed(-self.speedRotate)
time.sleep(self.rotateTime)
self.guidewirRotateMotor.set_speed(0)
def chuck_fasten(self):
self.gripperBack.gripper_chuck_fasten()
time.sleep(1)
self.guidewireRotateMotor.set_speed(self.speedRotate)
time.sleep(self.rotateTime)
self.guidewireRotateMotor.set_speed(0)
def draw_guidewire_advance(self):
"""
the shiftboard advance in the process of drawing back of guidewire
"""
self.gripperFront.gripper_chuck_loosen()
self.gripperBack.gripper_chuck_loosen()
time.sleep(1)
self.gripperFront.gripper_chuck_fasten()
self.gripperBack.gripper_chuck_fasten()
time.sleep(1)
self.guidewireRotateMotor.set_speed(-self.speedRotate)
time.sleep(self.rotateTime)
self.guidewireRotateMotor.set_speed(0)
self.guidewireProgressMotor.set_speed(self.speedProgress)
self.global_state = self.infraredReflectiveSensor.read_current_state()
while self.global_state != 2:
time.sleep(0.5)
self.global_state = self.infraredReflectiveSensor.read_current_state(
)
#print "advancing", self.global_state
#print "front limitation arrived"
self.guidewireProgressMotor.set_speed(0)
#self.gripperFront.gripper_chuck_fasten()
#self.gripperBack.gripper_chuck_fasten()
time.sleep(1)
#self.guidewireRotateMotor.rm_move_to_position(80, -4000)
#time.sleep(3)
# self.gripperFront.gripper_chuck_fasten()
# time.sleep(1)
#self.gripperFront.gripper_chuck_loosen()
#self.gripperBack.gripper_chuck_loosen()
#time.sleep(1)
def multitime_draw_back_guidewire(self):
"""
the process of drawing back guidewire for several times
"""
self.define_number_of_cycles()
for i in range(0, self.number_of_cycles):
self.draw_guidewire_advance()
self.draw_guidewire_back()
print(i)
def automatic_procedure(self):
self.angioMotor.set_pos_speed(4)
self.angioMotor.set_position(10)
self.angioMotor.push_contrast_media()
print "angiographing finish"
time.sleep(5)
self.multitime_push_guidewire()
def push_and_pull(self):
"""
the test of processing and drawing back guidewire for several times
"""
self.multitime_push_guidewire()
self.multitime_draw_back_guidewire()
def loosen(self):
"""
the test of gripper
"""
self.gripperBack.gripper_chuck_fasten()
time.sleep(1)
self.gripperBack.gripper_chuck_loosen()
#self.gripperBack.gripper_chuck_loosen()
time.sleep(1)
def catheter_advance(self):
"""
the process of guidewire and cathter advance by turns
"""
self.gripperFront.gripper_chuck_loosen()
self.gripperBack.gripper_chuck_loosen()
self.draw_back_guidewire_curcuit_flag == True
self.needToRetract = False
self.guidewireProgressMotor.set_speed(self.speedProgress)
self.catheterMotor.set_speed(self.speedCatheter)
self.global_state = self.infraredReflectiveSensor.read_current_state()
while self.global_state != 2:
time.sleep(0.5)
self.global_state = self.infraredReflectiveSensor.read_current_state(
)
#print "pushing", self.global_state
#print "front limitation arrived"
self.guidewireProgressMotor.set_speed(0)
self.catheterMotor.set_speed(0)
#self.guidewireRotateMotor.rm_move_to_position(90, -8000)
#time.sleep(4)
#self.context.clear()
self.draw_back_guidewire_curcuit_flag == False
#self.gripperFront.gripper_chuck_loosen()
#self.gripperBack.gripper_chuck_loosen()
#time.sleep(1)
# fasten front gripper
self.gripperFront.gripper_chuck_fasten()
# self-tightening chunck
self.gripperBack.gripper_chuck_fasten()
time.sleep(1)
self.guidewireRotateMotor.set_speed(-self.speedRotate) # +/loosen
time.sleep(self.rotateTime)
self.guidewireRotateMotor.set_speed(0)
#self.gripperFront.gripper_chuck_loosen()
#time.sleep(1)
self.guidewireProgressMotor.set_speed(-self.speedProgress)
self.global_state = self.infraredReflectiveSensor.read_current_state()
while self.global_state != 1:
time.sleep(0.5)
self.global_state = self.infraredReflectiveSensor.read_current_state(
)
print "retracting", self.global_state
print "back limitation arrived"
self.guidewireProgressMotor.set_speed(0)
self.guidewireRotateMotor.set_speed(self.speedRotate)
time.sleep(self.rotateTime)
self.guidewireRotateMotor.set_speed(0)
def multitime_catheter_advance(self):
"""
the process of guidewire and cathter advance by turns for several times
"""
self.define_number_of_cycles()
for i in range(0, self.number_of_cycles):
self.catheter_advance()
#print(i)
def test(self):
self.gripperBack.gripper_chuck_fasten()
def catheter_back(self):
"""
the process of guidewire and cathter by turns for several times
"""
self.define_number_of_cycles()
for i in range(0, self.number_of_cycles):
self.draw_guidewire_back()
self.catheterMotor.set_speed(self.speedCatheter)
print(i)
def initialization(self):
"""
the initialization of the status of robot
"""
self.draw_back_guidewire_curcuit_flag == False
#self.gripperFront.gripper_chuck_loosen()
#self.gripperBack.gripper_chuck_loosen()
#time.sleep(3)
# fasten front gripper
# self.gripperFront.gripper_chuck_fasten()
# self-tightening chunck
# self.gripperBack.gripper_chuck_fasten()
# time.sleep(1)
# self.guidewireRotateMotor.rm_move_to_position(90, 4000) # +/loosen
# time.sleep(3)
#self.gripperFront.gripper_chuck_loosen()
#time.sleep(1)
self.guidewireProgressMotor.set_speed(-self.speedProgress)
self.global_state = self.infraredReflectiveSensor.read_current_state()
while self.global_state != 1:
time.sleep(0.5)
self.global_state = self.infraredReflectiveSensor.read_current_state(
)
print "retracting", self.global_state
print "back limitation arrived"
self.guidewireProgressMotor.set_speed(0)
self.guidewireRotateMotor.set_speed(self.speedRotate)
time.sleep(self.rotateTime)
self.guidewireRotateMotor.set_speed(0)
self.gripperFront.gripper_chuck_loosen()
self.gripperBack.gripper_chuck_loosen()
self.draw_back_guidewire_curcuit_flag == True
self.needToRetract = False
def catheter(self):
self.catheterMotor.set_speed(self.speedCatheter)
def define_number_of_cycles(self):
"""
define the number of cycels of the robot operation
"""
self.number_of_cycles = input("please input the number of cycles")
def aquirefeedback_context(self):
while True:
feedbackMsg = FeedbackMsg()
forcevalue = self.forcefeedback.aquireForce()
torquevalue = self.torquefeedback.aquireForce()
forcedirection = 0
if forcevalue < 0:
forcedirection = 1
else:
forcedirection = 0
forcevalue = abs(forcevalue)
torquedirection = 0
if torquevalue < 0:
torquedirection = 1
else:
torquedirection = 0
torquevalue = abs(torquevalue)
feedbackMsg.set_force_direction(forcedirection)
feedbackMsg.set_force_value(forcevalue)
feedbackMsg.set_torque_direction(torquedirection)
feedbackMsg.set_torque_value(torquevalue)
self.context.append_latest_forcefeedback_msg(feedbackMsg)
import time
import serial
import socket
import os.path
import sys
from Gripper import Gripper
myGripper = Gripper()
PORT = '12345'
usbOptions = ['/dev/ttyUSB0', '/dev/ttyUSB1', '/dev/ttyUSB2']
usbOptions = filter(os.path.exists, usbOptions)
assert (len(usbOptions) == 1)
usbPort = usbOptions[0]
s = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
s.bind(('0.0.0.0', 12345))
s.listen(1)
client, _ = s.accept()
while True:
myGripper.gripperMove(3, 100, 0.1)
try:
usb = serial.Serial(usbPort, 9600, timeout=10)
usb.open()
time.sleep(2) # give the serial time to get opened
except serial.SerialException:
print("Could not connect to requested port")
time.sleep(1)
continue
while True:
#!/usr/bin/python
import socket
import sys
import os.path
import fcntl
import struct
import serial
import threading
import time
from Gripper import Gripper
# Move Specified Gripper to Specified Angle (-180, 180)
myGripper = Gripper()
# Make sure the script is run as sudo
if not os.geteuid() == 0:
sys.exit("\nOnly root can run this script\n")
udG = 0
rotG = 1
yG = 2
cG = 3
# Set up the USB connection to the Arduino. This will error
# if a USB cord is not connected.
usbOptions = ['/dev/ttyUSB0', '/dev/ttyUSB1', '/dev/ttyUSB2']
usbOptions = filter(os.path.exists, usbOptions)
assert(len(usbOptions) == 1)
usbPort = usbOptions[0]
#!/usr/bin/python
import socket
import sys
import os.path
import fcntl
import struct
import serial
import threading
import time
from Gripper import Gripper
# Move Specified Gripper to Specified Angle (-180, 180)
myGripper = Gripper()
# Make sure the script is run as sudo
if not os.geteuid() == 0:
sys.exit("\nOnly root can run this script\n")
udG = 0
rotG = 1
yG = 2
cG = 3
# Set up the USB connection to the Arduino. This will error
# if a USB cord is not connected.
usbOptions = ['/dev/ttyUSB0', '/dev/ttyUSB1', '/dev/ttyUSB2']
usbOptions = filter(os.path.exists, usbOptions)
assert (len(usbOptions) == 1)
usbPort = usbOptions[0]
from Gripper import Gripper
import os
if __name__ == '__main__':
myGripper = Gripper()
while True:
try:
myGripper.manualInput()
except KeyboardInterrupt:
myGripper.allOff()
os.system("sudo killall servod")
print("Closed Fine!")
break
