-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathRobotControl_func_ros1.py
180 lines (155 loc) · 6.12 KB
/
RobotControl_func_ros1.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
# import pythoncom
# pythoncom.CoInitialize()
import time
import random
from tkinter.tix import Tree
import numpy as np
from PyQt5.QtCore import QMutex, QObject, pyqtSlot, QThreadPool, QRunnable, QThread, pyqtSignal
from PyQt5.QtWidgets import QApplication
import math
import rospy
# sys.path.append(os.path.join(os.path.dirname(__file__), '../../../', 'tm_msgs/msg'))
# print(sys.path)
# from tm_msgs import msg
from tm_msgs.msg import *
from tm_msgs.srv import *
# Chong: for matrix Rotation 轉成常用的右邊x正前方y
cos145 = math.cos(math.radians(135))
sin145 = math.sin(math.radians(135))
R = np.array([[cos145,-sin145,0],[sin145,cos145,0],[0,0,1]])
mutex = QMutex()
# Robot Arm move
class myMitter(QObject):
done = pyqtSignal(bool)
class worker(QThread):
def __init__(self, pos, speed, line):
super(worker, self).__init__()
self.pos = pos
self.speed = speed
self.line = line
self.mitter = myMitter()
@pyqtSlot()
def run(self):
try:
mutex.lock()
rospy.wait_for_service('tm_driver/ask_sta')
rospy.wait_for_service('tm_driver/set_event')
rospy.wait_for_service('tm_driver/set_positions')
ask_sta = rospy.ServiceProxy('tm_driver/ask_sta', AskSta)
set_event = rospy.ServiceProxy('tm_driver/set_event', SetEvent)
set_positions = rospy.ServiceProxy('tm_driver/set_positions', SetPositions)
print(self.pos)
if self.line == False:
set_positions(SetPositionsRequest.PTP_T, self.pos, self.speed, 1, 0, False)
else:
set_positions(SetPositionsRequest.LINE_T, self.pos, self.speed, 0.5, 0, False)
set_event(SetEventRequest.TAG, 1, 0)
while True:
rospy.sleep(0.2)
res = ask_sta('01', str(1), 1)
if res.subcmd == '01':
data = res.subdata.split(',')
if data[1] == 'true':
rospy.loginfo('point %d (Tag %s) is reached', 1, data[0])
break
except Exception as e:
print(e)
# self.emitter.done.emit(False)
self.mitter.done.emit(True)
# print('emit')
mutex.unlock()
class RobotControl_Func():
def __init__(self):
super().__init__()
self.xPos = 0
self.yPos = 0
self.zPos = 0
self.Rx = 0
self.Ry = 0
self.Rz = 0
self.speed = 0
self.accel = 0
self.pool = QThreadPool.globalInstance()
# For Arm
self.pool.setMaxThreadCount(1)
# For Arm + AMM
# self.pool.setMaxThreadCount(2)
self.threadDone = True
def exit(self):
set_event = rospy.ServiceProxy('tm_driver/set_event', SetEvent)
set_event(SetEventRequest.EXIT, 1, 0)
def stop(self):
set_event = rospy.ServiceProxy('tm_driver/set_event', SetEvent)
set_event(SetEventRequest.STOP, 1, 0)
def resume(self):
set_event = rospy.ServiceProxy('tm_driver/set_event', SetEvent)
set_event(SetEventRequest.RESUME,1,0)
def pasue(self):
set_event = rospy.ServiceProxy('tm_driver/set_event', SetEvent)
set_event(SetEventRequest.PAUSE, 1, 0)
def on_worker_done(self, threadDone):
#print(threadDone)
print("True")
self.threadDone = True
def set_TMPos(self, pos, speed = 1, line = False):
# transself.set_TMPos_new(pos)form to TM robot coordinate
tmp = []
runnable = worker(tmp, speed, line)
cos135 = math.cos(math.radians(135))
sin135 = math.sin(math.radians(135))
R = np.array([[cos135,-sin135,0],[sin135,cos135,0],[0,0,1]])
ori_xyz = [pos[0],pos[1],pos[2]]
after_xyz = ori_xyz
tmp.append(after_xyz[0] / 1000)
tmp.append(after_xyz[1] / 1000)
tmp.append(after_xyz[2] / 1000)
#tmp.append(after_xyz[0]/1000)
#tmp.append(after_xyz[1]/1000)
#tmp.append(after_xyz[2]/1000)
# tmp.append(pos[3] * np.pi / 180)
# tmp.append(pos[4] * np.pi / 180)
# tmp.append(pos[5] * np.pi / 180)
tmp.append(pos[3])
tmp.append(pos[4])
tmp.append(pos[5])
self.threadDone = False
runnable = worker(tmp, speed, line)
runnable.mitter.done.connect(self.on_worker_done)
runnable.start()
runnable.wait()
def get_TMPos(self):
# listen to 'feedback_states' topic
data = rospy.wait_for_message("/feedback_states", FeedbackState, timeout=None)
print("$$$$$$$")
print(data.tool_pose)
print("$$$$$$$")
# print("$$$$$$$$$$$$$$$$$$$444")
# print(data.tcp_speed)
# cos45 = math.cos(math.radians(45))
# sin45 = math.sin(math.radians(45))
# R2 = np.array([[cos45,-sin45,0],[sin45,cos45,0],[0,0,1]])
cos135 = math.cos(math.radians(-135))
sin135 = math.sin(math.radians(-135))
R = np.array([[cos135,-sin135,0],[sin135,cos135,0],[0,0,1]])
current_pos = list(data.tool_pose)
ori_xyz = [current_pos[0],current_pos[1],current_pos[2]]
after_xyz = ori_xyz
current_pos[0] = after_xyz[0] * 1000
current_pos[1] = after_xyz[1] * 1000
current_pos[2] = after_xyz[2] * 1000
#current_pos[0] = current_pos[0] * 1000
#current_pos[1] = current_pos[1] * 1000
#current_pos[2] = current_pos[2] * 1000
# current_pos[3] = current_pos[3] * 180 / np.pi #出來是角度
# current_pos[4] = current_pos[4] * 180 / np.pi
# current_pos[5] = current_pos[5] * 180 / np.pi
current_pos[3] = current_pos[3]
current_pos[4] = current_pos[4]
current_pos[5] = current_pos[5]
# print(self.robot)
return current_pos
def get_TMJoint(self):
data = rospy.wait_for_message("/feedback_states", FeedbackState, timeout=None)
print("$$$$$$$")
print(data.joint_pos)
print("$$$$$$$")