car.py
Go to the documentation of this file.
1 #!/usr/bin/env python
2 # -*- coding: utf-8 -*-
3 # ##
4 # @brief [py demo] car demo [robotx6 sync test]
5 # @author Kab Kyoum Kim (kabkyoum.kim@doosan.com)
6 
7 import rospy
8 import os
9 import threading, time
10 import sys
11 sys.dont_write_bytecode = True
12 sys.path.append( os.path.abspath(os.path.join(os.path.dirname(__file__),"../../../../common/imp")) ) # get import pass : DSR_ROBOT.py
13 
14 
15 # for single robot
16 #import DR_init
17 #DR_init.__dsr__id = "dsr01"
18 #DR_init.__dsr__model = "m1013"
19 #from DSR_ROBOT import *
20 
21 # for mulit robot
22 ########from DSR_ROBOT_MULTI import *
23 from DSR_ROBOT import *
24 
25 NUM_ROBOT = 4
26 
27 ############################################################################################
28 class CRobotSync:
29  def __init__(self, r):
30  self.description = "Sync for Multiple Robos"
31  self.author = "Doosan Robotics"
32  self.nRobot = r
33  self.nIsRun = True
34 
35  self.nWaitBit = 0
36  self.nCurBit = 0
37 
38  self.bIsWait = list()
39  self.lock = list()
40 
41  for i in range(r):
42  self.lock.append( threading.Lock() )
43  self.bIsWait.append(False)
44  self.nWaitBit |= 0x1<<i
45 
46  def CleanUp(self):
47  if True == self.nIsRun:
48  self.nIsRun = False
49  print("~CleanUp()")
50 
51  def Wait(self, r):
52  self.bIsWait[r] = True
53  self.lock[r].acquire()
54  self.bIsWait[r] = False
55  return 0
56 
57  def WakeUp(self, r):
58  while self.nIsRun:
59  if(True == self.bIsWait[r]):
60  self.lock[r].release()
61  break;
62  else:
63  time.sleep(0.01)
64  return 0
65 
66  def WakeUpAll(self):
67  self.nCurBit = 0
68  while self.nIsRun:
69  for i in range(self.nRobot):
70  if(True == self.bIsWait[i]):
71  self.nCurBit |= 0x1<<i;
72  if(self.nWaitBit == self.nCurBit):
73  break;
74  for i in range(self.nRobot):
75  self.lock[i].release()
76  return 0
77 
78 RobotSync = CRobotSync(NUM_ROBOT)
79 
80 #######################################################################################
81 
82 
83 #----------------------------------------------------------------------
84 J0 = posj(0, 0, 0, 0, 0, 0)
85 J1 = posj(0, 0, 0, 30, 30, 0)
86 
87 JReady = [0, -20, 110, 0, 60, 0]
88 TCP_POS = [0, 0, 0, 0, 0, 0]
89 J00 = [-180, 0, -145, 0, -35, 0]
90 J01r = [-180.0, 71.4, -145.0, 0.0, -9.7, 0.0]
91 J02r = [-180.0, 67.7, -144.0, 0.0, 76.3, 0.0]
92 J03r = [-180.0, 0.0, 0.0, 0.0, 0.0, 0.0]
93 J04r = [-90.0, 0.0, 0.0, 0.0, 0.0, 0.0]
94 J04r1 = [-90.0, 30.0, -60.0, 0.0, 30.0, -0.0]
95 J04r2 = [-90.0, -45.0, 90.0, 0.0, -45.0, -0.0]
96 J04r3 = [-90.0, 60.0, -120.0, 0.0, 60.0, -0.0]
97 J04r4 = [-90.0, 0.0, -0.0, 0.0, 0.0, -0.0]
98 J05r = [-144.0, -4.0, -84.8, -90.9, 54.0, -1.1]
99 J07r = [-152.4, 12.4, -78.6, 18.7, -68.3, -37.7]
100 J08r = [-90.0, 30.0, -120.0, -90.0, -90.0, 0.0]
101 JEnd = [0.0, -12.6, 101.1, 0.0, 91.5, -0.0]
102 dREL1 = [0, 0, 350, 0, 0, 0]
103 dREL2 = [0, 0, -350, 0, 0, 0]
104 velx = [0, 0]
105 accx = [0, 0]
106 vel_spi = [400, 400]
107 acc_spi = [150, 150]
108 J1 = [81.2, 20.8, 127.8, 162.5, 56.1, -37.1]
109 X0 = [-88.7, 799.0, 182.3, 95.7, 93.7, 133.9]
110 X1 = [304.2, 871.8, 141.5, 99.5, 84.9, 133.4]
111 X2 = [437.1, 876.9, 362.1, 99.6, 84.0, 132.1]
112 X3 = [-57.9, 782.4, 478.4, 99.6, 84.0, 132.1]
113 amp = [0, 0, 0, 30, 30, 0]
114 period = [0, 0, 0, 3, 6, 0]
115 x01 = [423.6, 334.5, 651.2, 84.7, -180.0, 84.7]
116 x02 = [423.6, 34.5, 951.2, 68.2, -180.0, 68.2]
117 x03 = [423.6, -265.5, 651.2, 76.1, -180.0, 76.1]
118 x04 = [423.6, 34.5, 351.2, 81.3, -180.0, 81.3]
119 x0204c = [x02, x04]
120 
121 def thread_robot1(robot_id, robot_model):
122  try:
123  #nRobotID = 0
124  r = CDsrRobot(robot_id, robot_model)
125 
126  while not rospy.is_shutdown():
127  #RobotSync.Wait(nRobotID)
128  r.movej(J0, v=20, a=20)
129 
130  #RobotSync.Wait(nRobotID)
131  r.movej(J1, v=20, a=20)
132 
133  '''
134  while not rospy.is_shutdown():
135  RobotSync.Wait(nRobotID)
136  movej(JReady, v=20, a=20)
137 
138  RobotSync.Wait(nRobotID)
139  movej(J1, v=0, a=0, t=3)
140 
141  RobotSync.Wait(nRobotID)
142  movel(X3, velx, accx, t=2.5)
143 
144 
145  for i in range(0, 1):
146  RobotSync.Wait(nRobotID)
147  movel(X2, velx, accx, t=2.5, radius=50, ref=DR_BASE, mod=DR_MV_MOD_ABS)
148  RobotSync.Wait(nRobotID)
149  movel(X1, velx, accx, t=1.5, radius=50, ref=DR_BASE, mod=DR_MV_MOD_ABS)
150  RobotSync.Wait(nRobotID)
151  movel(X0, velx, accx, t=2.5)
152  RobotSync.Wait(nRobotID)
153  movel(X1, velx, accx, t=2.5, radius=50, ref=DR_BASE, mod=DR_MV_MOD_ABS)
154  RobotSync.Wait(nRobotID)
155  movel(X2, velx, accx, t=1.5, radius=50, ref=DR_BASE, mod=DR_MV_MOD_ABS)
156  RobotSync.Wait(nRobotID)
157  movel(X3, velx, accx, t=2.5, radius=50, ref=DR_BASE, mod=DR_MV_MOD_ABS)
158 
159  RobotSync.Wait(nRobotID)
160  movej(J00, v=60, a=60, t=6)
161 
162  RobotSync.Wait(nRobotID)
163  movej(J01r, v=0, a=0, t=2, radius=100, mod=DR_MV_MOD_ABS)
164 
165  RobotSync.Wait(nRobotID)
166  movej(J02r, v=0, a=0, t=2, radius=50, mod=DR_MV_MOD_ABS)
167 
168  RobotSync.Wait(nRobotID)
169  movej(J03r, v=0, a=0, t=2)
170 
171  RobotSync.Wait(nRobotID)
172  movej(J04r, v=0, a=0, t=1.5)
173 
174  RobotSync.Wait(nRobotID)
175  movej(J04r1, v=0, a=0, t=2, radius=50, mod=DR_MV_MOD_ABS)
176 
177  RobotSync.Wait(nRobotID)
178  movej(J04r2, v=0, a=0, t=4, radius=50, mod=DR_MV_MOD_ABS)
179 
180  RobotSync.Wait(nRobotID)
181  movej(J04r3, v=0, a=0, t=4, radius=50, mod=DR_MV_MOD_ABS)
182 
183  RobotSync.Wait(nRobotID)
184  movej(J04r4, v=0, a=0, t=2)
185 
186  RobotSync.Wait(nRobotID)
187  movej(J05r, v=0, a=0, t=2.5, radius=100, mod=DR_MV_MOD_ABS)
188 
189  RobotSync.Wait(nRobotID)
190  movel(dREL1, velx, accx, t=1, radius=50, ref=DR_TOOL, mod=DR_MV_MOD_ABS)
191 
192  RobotSync.Wait(nRobotID)
193  movel(dREL2, velx, accx, t=1.5, radius=100, ref=DR_TOOL, mod=DR_MV_MOD_ABS)
194 
195  RobotSync.Wait(nRobotID)
196  movej(J07r, v=60, a=60, t=1.5, radius=100, mod=DR_MV_MOD_ABS)
197 
198  RobotSync.Wait(nRobotID)
199  movej(J08r, v=60, a=60, t=2)
200 
201  RobotSync.Wait(nRobotID)
202  movej(JEnd, v=60, a=60, t=4)
203 
204  RobotSync.Wait(nRobotID)
205  move_periodic(amp, period, 0, 1, ref=DR_TOOL)
206 
207  RobotSync.Wait(nRobotID)
208  move_spiral(rev=3, rmax=200, lmax=100, v=vel_spi, a=acc_spi, t=0, axis=DR_AXIS_X, ref=DR_TOOL)
209 
210  RobotSync.Wait(nRobotID)
211  movel(x01, velx, accx, t=2)
212 
213  RobotSync.Wait(nRobotID)
214  movel(x04, velx, accx, t=2, radius=100, ref=DR_BASE, mod=DR_MV_MOD_ABS)
215 
216  RobotSync.Wait(nRobotID)
217  movel(x03, velx, accx, t=2, radius=100, ref=DR_BASE, mod=DR_MV_MOD_ABS)
218 
219  RobotSync.Wait(nRobotID)
220  movel(x02, velx, accx, t=2, radius=100, ref=DR_BASE, mod=DR_MV_MOD_ABS)
221 
222  RobotSync.Wait(nRobotID)
223  movel(x01, velx, accx, t=2)
224 
225  RobotSync.Wait(nRobotID)
226  movec(pos1=x02, pos2=x04, v=velx, a=accx, t=4, radius=360, mod=DR_MV_MOD_ABS, ref=DR_BASE)
227  '''
228 
229  except Exception as err:
230  RobotSync.CleanUp()
231  rospy.loginfo("Runtime Exception : %s" % err)
232  return 0
233 
234 def thread_robot2(robot_id, robot_model):
235  try:
236  nRobotID = 0
237  r = CDsrRobot(robot_id, robot_model)
238 
239  '''
240  while not rospy.is_shutdown():
241  RobotSync.Wait(nRobotID)
242  r.movej(J0, v=20, a=20)
243  RobotSync.Wait(nRobotID)
244  r.movej(J1, v=20, a=20)
245  '''
246 
247  while not rospy.is_shutdown():
248  RobotSync.Wait(nRobotID)
249  r.movej(JReady, v=20, a=20)
250 
251  RobotSync.Wait(nRobotID)
252  r.movej(J1, v=0, a=0, t=3)
253 
254  RobotSync.Wait(nRobotID)
255  r.movel(X3, velx, accx, t=2.5)
256 
257  for i in range(0, 1):
258  RobotSync.Wait(nRobotID)
259  r.movel(X2, velx, accx, t=2.5, radius=50, ref=DR_BASE, mod=DR_MV_MOD_ABS)
260  RobotSync.Wait(nRobotID)
261  r.movel(X1, velx, accx, t=1.5, radius=50, ref=DR_BASE, mod=DR_MV_MOD_ABS)
262  RobotSync.Wait(nRobotID)
263  r.movel(X0, velx, accx, t=2.5)
264  RobotSync.Wait(nRobotID)
265  r.movel(X1, velx, accx, t=2.5, radius=50, ref=DR_BASE, mod=DR_MV_MOD_ABS)
266  RobotSync.Wait(nRobotID)
267  r.movel(X2, velx, accx, t=1.5, radius=50, ref=DR_BASE, mod=DR_MV_MOD_ABS)
268  RobotSync.Wait(nRobotID)
269  r.movel(X3, velx, accx, t=2.5, radius=50, ref=DR_BASE, mod=DR_MV_MOD_ABS)
270 
271  RobotSync.Wait(nRobotID)
272  r.movej(J00, v=60, a=60, t=6)
273 
274  RobotSync.Wait(nRobotID)
275  r.movej(J01r, v=0, a=0, t=2, radius=100, mod=DR_MV_MOD_ABS)
276 
277  RobotSync.Wait(nRobotID)
278  r.movej(J02r, v=0, a=0, t=2, radius=50, mod=DR_MV_MOD_ABS)
279 
280  RobotSync.Wait(nRobotID)
281  r.movej(J03r, v=0, a=0, t=2)
282 
283  RobotSync.Wait(nRobotID)
284  r.movej(J04r, v=0, a=0, t=1.5)
285 
286  RobotSync.Wait(nRobotID)
287  r.movej(J04r1, v=0, a=0, t=2, radius=50, mod=DR_MV_MOD_ABS)
288 
289  RobotSync.Wait(nRobotID)
290  r.movej(J04r2, v=0, a=0, t=4, radius=50, mod=DR_MV_MOD_ABS)
291 
292  RobotSync.Wait(nRobotID)
293  r.movej(J04r3, v=0, a=0, t=4, radius=50, mod=DR_MV_MOD_ABS)
294 
295  RobotSync.Wait(nRobotID)
296  r.movej(J04r4, v=0, a=0, t=2)
297 
298  RobotSync.Wait(nRobotID)
299  r.movej(J05r, v=0, a=0, t=2.5, radius=100, mod=DR_MV_MOD_ABS)
300 
301  RobotSync.Wait(nRobotID)
302  r.movel(dREL1, velx, accx, t=1, radius=50, ref=DR_TOOL, mod=DR_MV_MOD_ABS)
303 
304  RobotSync.Wait(nRobotID)
305  r.movel(dREL2, velx, accx, t=1.5, radius=100, ref=DR_TOOL, mod=DR_MV_MOD_ABS)
306 
307  RobotSync.Wait(nRobotID)
308  r.movej(J07r, v=60, a=60, t=1.5, radius=100, mod=DR_MV_MOD_ABS)
309 
310  RobotSync.Wait(nRobotID)
311  r.movej(J08r, v=60, a=60, t=2)
312 
313  RobotSync.Wait(nRobotID)
314  r.movej(JEnd, v=60, a=60, t=4)
315 
316  RobotSync.Wait(nRobotID)
317  r.move_periodic(amp, period, 0, 1, ref=DR_TOOL)
318 
319  RobotSync.Wait(nRobotID)
320  r.move_spiral(rev=3, rmax=200, lmax=100, v=vel_spi, a=acc_spi, t=0, axis=DR_AXIS_X, ref=DR_TOOL)
321 
322  RobotSync.Wait(nRobotID)
323  r.movel(x01, velx, accx, t=2)
324 
325  RobotSync.Wait(nRobotID)
326  r.movel(x04, velx, accx, t=2, radius=100, ref=DR_BASE, mod=DR_MV_MOD_ABS)
327 
328  RobotSync.Wait(nRobotID)
329  r.movel(x03, velx, accx, t=2, radius=100, ref=DR_BASE, mod=DR_MV_MOD_ABS)
330 
331  RobotSync.Wait(nRobotID)
332  r.movel(x02, velx, accx, t=2, radius=100, ref=DR_BASE, mod=DR_MV_MOD_ABS)
333 
334  RobotSync.Wait(nRobotID)
335  r.movel(x01, velx, accx, t=2)
336 
337  RobotSync.Wait(nRobotID)
338  r.movec(pos1=x02, pos2=x04, v=velx, a=accx, t=4, radius=360, mod=DR_MV_MOD_ABS, ref=DR_BASE)
339 
340  except Exception as err:
341  RobotSync.CleanUp()
342  rospy.loginfo("Runtime Exception : %s" % err)
343  return 0
344 
345 def thread_robot3(robot_id, robot_model):
346  try:
347  #nRobotID = 2
348  r = CDsrRobot(robot_id, robot_model)
349 
350 
351  while not rospy.is_shutdown():
352  # RobotSync.Wait(nRobotID)
353  r.movej(J0, v=20, a=20)
354  # RobotSync.Wait(nRobotID)
355  r.movej(J1, v=20, a=20)
356 
357  '''
358  while not rospy.is_shutdown():
359  RobotSync.Wait(nRobotID)
360  movej(JReady, v=20, a=20)
361 
362  RobotSync.Wait(nRobotID)
363  movej(J1, v=0, a=0, t=3)
364 
365  RobotSync.Wait(nRobotID)
366  movel(X3, velx, accx, t=2.5)
367 
368  for i in range(0, 1):
369  RobotSync.Wait(nRobotID)
370  movel(X2, velx, accx, t=2.5, radius=50, ref=DR_BASE, mod=DR_MV_MOD_ABS)
371  RobotSync.Wait(nRobotID)
372  movel(X1, velx, accx, t=1.5, radius=50, ref=DR_BASE, mod=DR_MV_MOD_ABS)
373  RobotSync.Wait(nRobotID)
374  movel(X0, velx, accx, t=2.5)
375  RobotSync.Wait(nRobotID)
376  movel(X1, velx, accx, t=2.5, radius=50, ref=DR_BASE, mod=DR_MV_MOD_ABS)
377  RobotSync.Wait(nRobotID)
378  movel(X2, velx, accx, t=1.5, radius=50, ref=DR_BASE, mod=DR_MV_MOD_ABS)
379  RobotSync.Wait(nRobotID)
380  movel(X3, velx, accx, t=2.5, radius=50, ref=DR_BASE, mod=DR_MV_MOD_ABS)
381 
382  RobotSync.Wait(nRobotID)
383  movej(J00, v=60, a=60, t=6)
384 
385  RobotSync.Wait(nRobotID)
386  movej(J01r, v=0, a=0, t=2, radius=100, mod=DR_MV_MOD_ABS)
387 
388  RobotSync.Wait(nRobotID)
389  movej(J02r, v=0, a=0, t=2, radius=50, mod=DR_MV_MOD_ABS)
390 
391  RobotSync.Wait(nRobotID)
392  movej(J03r, v=0, a=0, t=2)
393 
394  RobotSync.Wait(nRobotID)
395  movej(J04r, v=0, a=0, t=1.5)
396 
397  RobotSync.Wait(nRobotID)
398  movej(J04r1, v=0, a=0, t=2, radius=50, mod=DR_MV_MOD_ABS)
399 
400  RobotSync.Wait(nRobotID)
401  movej(J04r2, v=0, a=0, t=4, radius=50, mod=DR_MV_MOD_ABS)
402 
403  RobotSync.Wait(nRobotID)
404  movej(J04r3, v=0, a=0, t=4, radius=50, mod=DR_MV_MOD_ABS)
405 
406  RobotSync.Wait(nRobotID)
407  movej(J04r4, v=0, a=0, t=2)
408 
409  RobotSync.Wait(nRobotID)
410  movej(J05r, v=0, a=0, t=2.5, radius=100, mod=DR_MV_MOD_ABS)
411 
412  RobotSync.Wait(nRobotID)
413  movel(dREL1, velx, accx, t=1, radius=50, ref=DR_TOOL, mod=DR_MV_MOD_ABS)
414 
415  RobotSync.Wait(nRobotID)
416  movel(dREL2, velx, accx, t=1.5, radius=100, ref=DR_TOOL, mod=DR_MV_MOD_ABS)
417 
418  RobotSync.Wait(nRobotID)
419  movej(J07r, v=60, a=60, t=1.5, radius=100, mod=DR_MV_MOD_ABS)
420 
421  RobotSync.Wait(nRobotID)
422  movej(J08r, v=60, a=60, t=2)
423 
424  RobotSync.Wait(nRobotID)
425  movej(JEnd, v=60, a=60, t=4)
426 
427  RobotSync.Wait(nRobotID)
428  move_periodic(amp, period, 0, 1, ref=DR_TOOL)
429 
430  RobotSync.Wait(nRobotID)
431  move_spiral(rev=3, rmax=200, lmax=100, v=vel_spi, a=acc_spi, t=0, axis=DR_AXIS_X, ref=DR_TOOL)
432 
433  RobotSync.Wait(nRobotID)
434  movel(x01, velx, accx, t=2)
435 
436  RobotSync.Wait(nRobotID)
437  movel(x04, velx, accx, t=2, radius=100, ref=DR_BASE, mod=DR_MV_MOD_ABS)
438 
439  RobotSync.Wait(nRobotID)
440  movel(x03, velx, accx, t=2, radius=100, ref=DR_BASE, mod=DR_MV_MOD_ABS)
441 
442  RobotSync.Wait(nRobotID)
443  movel(x02, velx, accx, t=2, radius=100, ref=DR_BASE, mod=DR_MV_MOD_ABS)
444 
445  RobotSync.Wait(nRobotID)
446  movel(x01, velx, accx, t=2)
447 
448  RobotSync.Wait(nRobotID)
449  movec(pos1=x02, pos2=x04, v=velx, a=accx, t=4, radius=360, mod=DR_MV_MOD_ABS, ref=DR_BASE)
450  '''
451 
452  except Exception as err:
453  RobotSync.CleanUp()
454  rospy.loginfo("Runtime Exception : %s" % err)
455  return 0
456 
457 def thread_robot4(robot_id, robot_model):
458  try:
459  nRobotID = 1
460  r = CDsrRobot(robot_id, robot_model)
461 
462  '''
463  while not rospy.is_shutdown():
464  RobotSync.Wait(nRobotID)
465  r.movej(J0, v=20, a=20)
466  RobotSync.Wait(nRobotID)
467  r.movej(J1, v=20, a=20)
468  '''
469 
470  while not rospy.is_shutdown():
471  RobotSync.Wait(nRobotID)
472  r.movej(JReady, v=20, a=20)
473 
474  RobotSync.Wait(nRobotID)
475  r.movej(J1, v=0, a=0, t=3)
476 
477  RobotSync.Wait(nRobotID)
478  r.movel(X3, velx, accx, t=2.5)
479 
480  for i in range(0, 1):
481  RobotSync.Wait(nRobotID)
482  r.movel(X2, velx, accx, t=2.5, radius=50, ref=DR_BASE, mod=DR_MV_MOD_ABS)
483  RobotSync.Wait(nRobotID)
484  r.movel(X1, velx, accx, t=1.5, radius=50, ref=DR_BASE, mod=DR_MV_MOD_ABS)
485  RobotSync.Wait(nRobotID)
486  r.movel(X0, velx, accx, t=2.5)
487  RobotSync.Wait(nRobotID)
488  r.movel(X1, velx, accx, t=2.5, radius=50, ref=DR_BASE, mod=DR_MV_MOD_ABS)
489  RobotSync.Wait(nRobotID)
490  r.movel(X2, velx, accx, t=1.5, radius=50, ref=DR_BASE, mod=DR_MV_MOD_ABS)
491  RobotSync.Wait(nRobotID)
492  r.movel(X3, velx, accx, t=2.5, radius=50, ref=DR_BASE, mod=DR_MV_MOD_ABS)
493 
494  RobotSync.Wait(nRobotID)
495  r.movej(J00, v=60, a=60, t=6)
496 
497  RobotSync.Wait(nRobotID)
498  r.movej(J01r, v=0, a=0, t=2, radius=100, mod=DR_MV_MOD_ABS)
499 
500  RobotSync.Wait(nRobotID)
501  r.movej(J02r, v=0, a=0, t=2, radius=50, mod=DR_MV_MOD_ABS)
502 
503  RobotSync.Wait(nRobotID)
504  r.movej(J03r, v=0, a=0, t=2)
505 
506  RobotSync.Wait(nRobotID)
507  r.movej(J04r, v=0, a=0, t=1.5)
508 
509  RobotSync.Wait(nRobotID)
510  r.movej(J04r1, v=0, a=0, t=2, radius=50, mod=DR_MV_MOD_ABS)
511 
512  RobotSync.Wait(nRobotID)
513  r.movej(J04r2, v=0, a=0, t=4, radius=50, mod=DR_MV_MOD_ABS)
514 
515  RobotSync.Wait(nRobotID)
516  r.movej(J04r3, v=0, a=0, t=4, radius=50, mod=DR_MV_MOD_ABS)
517 
518  RobotSync.Wait(nRobotID)
519  r.movej(J04r4, v=0, a=0, t=2)
520 
521  RobotSync.Wait(nRobotID)
522  r.movej(J05r, v=0, a=0, t=2.5, radius=100, mod=DR_MV_MOD_ABS)
523 
524  RobotSync.Wait(nRobotID)
525  r.movel(dREL1, velx, accx, t=1, radius=50, ref=DR_TOOL, mod=DR_MV_MOD_ABS)
526 
527  RobotSync.Wait(nRobotID)
528  r.movel(dREL2, velx, accx, t=1.5, radius=100, ref=DR_TOOL, mod=DR_MV_MOD_ABS)
529 
530  RobotSync.Wait(nRobotID)
531  r.movej(J07r, v=60, a=60, t=1.5, radius=100, mod=DR_MV_MOD_ABS)
532 
533  RobotSync.Wait(nRobotID)
534  r.movej(J08r, v=60, a=60, t=2)
535 
536  RobotSync.Wait(nRobotID)
537  r.movej(JEnd, v=60, a=60, t=4)
538 
539  RobotSync.Wait(nRobotID)
540  r.move_periodic(amp, period, 0, 1, ref=DR_TOOL)
541 
542  RobotSync.Wait(nRobotID)
543  r.move_spiral(rev=3, rmax=200, lmax=100, v=vel_spi, a=acc_spi, t=0, axis=DR_AXIS_X, ref=DR_TOOL)
544 
545  RobotSync.Wait(nRobotID)
546  r.movel(x01, velx, accx, t=2)
547 
548  RobotSync.Wait(nRobotID)
549  r.movel(x04, velx, accx, t=2, radius=100, ref=DR_BASE, mod=DR_MV_MOD_ABS)
550 
551  RobotSync.Wait(nRobotID)
552  r.movel(x03, velx, accx, t=2, radius=100, ref=DR_BASE, mod=DR_MV_MOD_ABS)
553 
554  RobotSync.Wait(nRobotID)
555  r.movel(x02, velx, accx, t=2, radius=100, ref=DR_BASE, mod=DR_MV_MOD_ABS)
556 
557  RobotSync.Wait(nRobotID)
558  r.movel(x01, velx, accx, t=2)
559 
560  RobotSync.Wait(nRobotID)
561  r.movec(pos1=x02, pos2=x04, v=velx, a=accx, t=4, radius=360, mod=DR_MV_MOD_ABS, ref=DR_BASE)
562 
563  except Exception as err:
564  RobotSync.CleanUp()
565  rospy.loginfo("Runtime Exception : %s" % err)
566  return 0
567 
568 def thread_robot5(robot_id, robot_model):
569  try:
570  nRobotID = 2
571  r = CDsrRobot(robot_id, robot_model)
572 
573  '''
574  while not rospy.is_shutdown():
575  RobotSync.Wait(nRobotID)
576  r.movej(J0, v=20, a=20)
577  RobotSync.Wait(nRobotID)
578  r.movej(J1, v=20, a=20)
579  '''
580 
581  while not rospy.is_shutdown():
582  RobotSync.Wait(nRobotID)
583  r.movej(JReady, v=20, a=20)
584 
585  RobotSync.Wait(nRobotID)
586  r.movej(J1, v=0, a=0, t=3)
587 
588  RobotSync.Wait(nRobotID)
589  r.movel(X3, velx, accx, t=2.5)
590 
591  for i in range(0, 1):
592  RobotSync.Wait(nRobotID)
593  r.movel(X2, velx, accx, t=2.5, radius=50, ref=DR_BASE, mod=DR_MV_MOD_ABS)
594  RobotSync.Wait(nRobotID)
595  r.movel(X1, velx, accx, t=1.5, radius=50, ref=DR_BASE, mod=DR_MV_MOD_ABS)
596  RobotSync.Wait(nRobotID)
597  r.movel(X0, velx, accx, t=2.5)
598  RobotSync.Wait(nRobotID)
599  r.movel(X1, velx, accx, t=2.5, radius=50, ref=DR_BASE, mod=DR_MV_MOD_ABS)
600  RobotSync.Wait(nRobotID)
601  r.movel(X2, velx, accx, t=1.5, radius=50, ref=DR_BASE, mod=DR_MV_MOD_ABS)
602  RobotSync.Wait(nRobotID)
603  r.movel(X3, velx, accx, t=2.5, radius=50, ref=DR_BASE, mod=DR_MV_MOD_ABS)
604 
605  RobotSync.Wait(nRobotID)
606  r.movej(J00, v=60, a=60, t=6)
607 
608  RobotSync.Wait(nRobotID)
609  r.movej(J01r, v=0, a=0, t=2, radius=100, mod=DR_MV_MOD_ABS)
610 
611  RobotSync.Wait(nRobotID)
612  r.movej(J02r, v=0, a=0, t=2, radius=50, mod=DR_MV_MOD_ABS)
613 
614  RobotSync.Wait(nRobotID)
615  r.movej(J03r, v=0, a=0, t=2)
616 
617  RobotSync.Wait(nRobotID)
618  r.movej(J04r, v=0, a=0, t=1.5)
619 
620  RobotSync.Wait(nRobotID)
621  r.movej(J04r1, v=0, a=0, t=2, radius=50, mod=DR_MV_MOD_ABS)
622 
623  RobotSync.Wait(nRobotID)
624  r.movej(J04r2, v=0, a=0, t=4, radius=50, mod=DR_MV_MOD_ABS)
625 
626  RobotSync.Wait(nRobotID)
627  r.movej(J04r3, v=0, a=0, t=4, radius=50, mod=DR_MV_MOD_ABS)
628 
629  RobotSync.Wait(nRobotID)
630  r.movej(J04r4, v=0, a=0, t=2)
631 
632  RobotSync.Wait(nRobotID)
633  r.movej(J05r, v=0, a=0, t=2.5, radius=100, mod=DR_MV_MOD_ABS)
634 
635  RobotSync.Wait(nRobotID)
636  r.movel(dREL1, velx, accx, t=1, radius=50, ref=DR_TOOL, mod=DR_MV_MOD_ABS)
637 
638  RobotSync.Wait(nRobotID)
639  r.movel(dREL2, velx, accx, t=1.5, radius=100, ref=DR_TOOL, mod=DR_MV_MOD_ABS)
640 
641  RobotSync.Wait(nRobotID)
642  r.movej(J07r, v=60, a=60, t=1.5, radius=100, mod=DR_MV_MOD_ABS)
643 
644  RobotSync.Wait(nRobotID)
645  r.movej(J08r, v=60, a=60, t=2)
646 
647  RobotSync.Wait(nRobotID)
648  r.movej(JEnd, v=60, a=60, t=4)
649 
650  RobotSync.Wait(nRobotID)
651  r.move_periodic(amp, period, 0, 1, ref=DR_TOOL)
652 
653  RobotSync.Wait(nRobotID)
654  r.move_spiral(rev=3, rmax=200, lmax=100, v=vel_spi, a=acc_spi, t=0, axis=DR_AXIS_X, ref=DR_TOOL)
655 
656  RobotSync.Wait(nRobotID)
657  r.movel(x01, velx, accx, t=2)
658 
659  RobotSync.Wait(nRobotID)
660  r.movel(x04, velx, accx, t=2, radius=100, ref=DR_BASE, mod=DR_MV_MOD_ABS)
661 
662  RobotSync.Wait(nRobotID)
663  r.movel(x03, velx, accx, t=2, radius=100, ref=DR_BASE, mod=DR_MV_MOD_ABS)
664 
665  RobotSync.Wait(nRobotID)
666  r.movel(x02, velx, accx, t=2, radius=100, ref=DR_BASE, mod=DR_MV_MOD_ABS)
667 
668  RobotSync.Wait(nRobotID)
669  r.movel(x01, velx, accx, t=2)
670 
671  RobotSync.Wait(nRobotID)
672  r.movec(pos1=x02, pos2=x04, v=velx, a=accx, t=4, radius=360, mod=DR_MV_MOD_ABS, ref=DR_BASE)
673 
674  except Exception as err:
675  RobotSync.CleanUp()
676  rospy.loginfo("Runtime Exception : %s" % err)
677  return 0
678 
679 def thread_robot6(robot_id, robot_model):
680  try:
681  nRobotID = 3
682  r = CDsrRobot(robot_id, robot_model)
683 
684  '''
685  while not rospy.is_shutdown():
686  RobotSync.Wait(nRobotID)
687  r.movej(J0, v=20, a=20)
688  RobotSync.Wait(nRobotID)
689  r.movej(J1, v=20, a=20)
690  '''
691 
692  while not rospy.is_shutdown():
693  RobotSync.Wait(nRobotID)
694  r.movej(JReady, v=20, a=20)
695 
696  RobotSync.Wait(nRobotID)
697  r.movej(J1, v=0, a=0, t=3)
698 
699  RobotSync.Wait(nRobotID)
700  r.movel(X3, velx, accx, t=2.5)
701 
702  for i in range(0, 1):
703  RobotSync.Wait(nRobotID)
704  r.movel(X2, velx, accx, t=2.5, radius=50, ref=DR_BASE, mod=DR_MV_MOD_ABS)
705  RobotSync.Wait(nRobotID)
706  r.movel(X1, velx, accx, t=1.5, radius=50, ref=DR_BASE, mod=DR_MV_MOD_ABS)
707  RobotSync.Wait(nRobotID)
708  r.movel(X0, velx, accx, t=2.5)
709  RobotSync.Wait(nRobotID)
710  r.movel(X1, velx, accx, t=2.5, radius=50, ref=DR_BASE, mod=DR_MV_MOD_ABS)
711  RobotSync.Wait(nRobotID)
712  r.movel(X2, velx, accx, t=1.5, radius=50, ref=DR_BASE, mod=DR_MV_MOD_ABS)
713  RobotSync.Wait(nRobotID)
714  r.movel(X3, velx, accx, t=2.5, radius=50, ref=DR_BASE, mod=DR_MV_MOD_ABS)
715 
716  RobotSync.Wait(nRobotID)
717  r.movej(J00, v=60, a=60, t=6)
718 
719  RobotSync.Wait(nRobotID)
720  r.movej(J01r, v=0, a=0, t=2, radius=100, mod=DR_MV_MOD_ABS)
721 
722  RobotSync.Wait(nRobotID)
723  r.movej(J02r, v=0, a=0, t=2, radius=50, mod=DR_MV_MOD_ABS)
724 
725  RobotSync.Wait(nRobotID)
726  r.movej(J03r, v=0, a=0, t=2)
727 
728  RobotSync.Wait(nRobotID)
729  r.movej(J04r, v=0, a=0, t=1.5)
730 
731  RobotSync.Wait(nRobotID)
732  r.movej(J04r1, v=0, a=0, t=2, radius=50, mod=DR_MV_MOD_ABS)
733 
734  RobotSync.Wait(nRobotID)
735  r.movej(J04r2, v=0, a=0, t=4, radius=50, mod=DR_MV_MOD_ABS)
736 
737  RobotSync.Wait(nRobotID)
738  r.movej(J04r3, v=0, a=0, t=4, radius=50, mod=DR_MV_MOD_ABS)
739 
740  RobotSync.Wait(nRobotID)
741  r.movej(J04r4, v=0, a=0, t=2)
742 
743  RobotSync.Wait(nRobotID)
744  r.movej(J05r, v=0, a=0, t=2.5, radius=100, mod=DR_MV_MOD_ABS)
745 
746  RobotSync.Wait(nRobotID)
747  r.movel(dREL1, velx, accx, t=1, radius=50, ref=DR_TOOL, mod=DR_MV_MOD_ABS)
748 
749  RobotSync.Wait(nRobotID)
750  r.movel(dREL2, velx, accx, t=1.5, radius=100, ref=DR_TOOL, mod=DR_MV_MOD_ABS)
751 
752  RobotSync.Wait(nRobotID)
753  r.movej(J07r, v=60, a=60, t=1.5, radius=100, mod=DR_MV_MOD_ABS)
754 
755  RobotSync.Wait(nRobotID)
756  r.movej(J08r, v=60, a=60, t=2)
757 
758  RobotSync.Wait(nRobotID)
759  r.movej(JEnd, v=60, a=60, t=4)
760 
761  RobotSync.Wait(nRobotID)
762  r.move_periodic(amp, period, 0, 1, ref=DR_TOOL)
763 
764  RobotSync.Wait(nRobotID)
765  r.move_spiral(rev=3, rmax=200, lmax=100, v=vel_spi, a=acc_spi, t=0, axis=DR_AXIS_X, ref=DR_TOOL)
766 
767  RobotSync.Wait(nRobotID)
768  r.movel(x01, velx, accx, t=2)
769 
770  RobotSync.Wait(nRobotID)
771  r.movel(x04, velx, accx, t=2, radius=100, ref=DR_BASE, mod=DR_MV_MOD_ABS)
772 
773  RobotSync.Wait(nRobotID)
774  r.movel(x03, velx, accx, t=2, radius=100, ref=DR_BASE, mod=DR_MV_MOD_ABS)
775 
776  RobotSync.Wait(nRobotID)
777  r.movel(x02, velx, accx, t=2, radius=100, ref=DR_BASE, mod=DR_MV_MOD_ABS)
778 
779  RobotSync.Wait(nRobotID)
780  r.movel(x01, velx, accx, t=2)
781 
782  RobotSync.Wait(nRobotID)
783  r.movec(pos1=x02, pos2=x04, v=velx, a=accx, t=4, radius=360, mod=DR_MV_MOD_ABS, ref=DR_BASE)
784 
785  except Exception as err:
786  RobotSync.CleanUp()
787  rospy.loginfo("Runtime Exception : %s" % err)
788  return 0
789 
790 def shutdown():
791 
792  print "shutdown time!"
793  print "shutdown time!"
794  print "shutdown time!"
795 
796  pub_stop_r1.publish(stop_mode=STOP_TYPE_QUICK)
797  pub_stop_r2.publish(stop_mode=STOP_TYPE_QUICK)
798  pub_stop_r3.publish(stop_mode=STOP_TYPE_QUICK)
799  pub_stop_r4.publish(stop_mode=STOP_TYPE_QUICK)
800  pub_stop_r5.publish(stop_mode=STOP_TYPE_QUICK)
801  pub_stop_r6.publish(stop_mode=STOP_TYPE_QUICK)
802 
803  return 0
804 
805 if __name__ == "__main__":
806  rospy.init_node('car_py')
807  rospy.on_shutdown(shutdown)
808 
809  robot_id1 = "dsr01"; robot_model1 = "m0617"
810  robot_id2 = "dsr02"; robot_model2 = "m1013"
811  robot_id3 = "dsr03"; robot_model3 = "m1509"
812  robot_id4 = "dsr04"; robot_model4 = "m1013"
813  robot_id5 = "dsr05"; robot_model5 = "m1013"
814  robot_id6 = "dsr06"; robot_model6 = "m1013"
815 
816  pub_stop_r1 = rospy.Publisher('/'+ robot_id1 + robot_model1 +'/stop', RobotStop, queue_size=10)
817  pub_stop_r2 = rospy.Publisher('/'+ robot_id2 + robot_model2 +'/stop', RobotStop, queue_size=10)
818  pub_stop_r3 = rospy.Publisher('/'+ robot_id3 + robot_model1 +'/stop', RobotStop, queue_size=10)
819  pub_stop_r4 = rospy.Publisher('/'+ robot_id4 + robot_model2 +'/stop', RobotStop, queue_size=10)
820  pub_stop_r5 = rospy.Publisher('/'+ robot_id5 + robot_model1 +'/stop', RobotStop, queue_size=10)
821  pub_stop_r6 = rospy.Publisher('/'+ robot_id6 + robot_model2 +'/stop', RobotStop, queue_size=10)
822 
823  #RobotSync = CRobotSync(NUM_ROBOT)
824 
825  t1 = threading.Thread(target=thread_robot1, args=(robot_id1, robot_model1))
826  t1.daemon = True
827  t1.start()
828 
829  t2 = threading.Thread(target=thread_robot2, args=(robot_id2, robot_model2))
830  t2.daemon = True
831  t2.start()
832 
833  t3 = threading.Thread(target=thread_robot3, args=(robot_id3, robot_model3))
834  t3.daemon = True
835  t3.start()
836 
837  t4 = threading.Thread(target=thread_robot4, args=(robot_id4, robot_model4))
838  t4.daemon = True
839  t4.start()
840 
841  t5 = threading.Thread(target=thread_robot5, args=(robot_id5, robot_model5))
842  t5.daemon = True
843  t5.start()
844 
845  t6 = threading.Thread(target=thread_robot6, args=(robot_id6, robot_model6))
846  t6.daemon = True
847  t6.start()
848 
849  time.sleep(5)
850 
851  while not rospy.is_shutdown():
852  time.sleep(0.01)
853  RobotSync.WakeUpAll()
854 
855  #----------------------------------------------------------------------
856 
857  print 'good bye!'
car.thread_robot1
def thread_robot1(robot_id, robot_model)
Definition: car.py:121
car.thread_robot4
def thread_robot4(robot_id, robot_model)
Definition: car.py:457
car.CRobotSync.WakeUp
def WakeUp(self, r)
Definition: car.py:57
car.CRobotSync.Wait
def Wait(self, r)
Definition: car.py:51
car.CRobotSync.__init__
def __init__(self, r)
Definition: car.py:29
car.thread_robot2
def thread_robot2(robot_id, robot_model)
Definition: car.py:234
car.shutdown
def shutdown()
Definition: car.py:790
car.CRobotSync.CleanUp
def CleanUp(self)
Definition: car.py:46
car.thread_robot5
def thread_robot5(robot_id, robot_model)
Definition: car.py:568
car.thread_robot6
def thread_robot6(robot_id, robot_model)
Definition: car.py:679
car.thread_robot3
def thread_robot3(robot_id, robot_model)
Definition: car.py:345
car.CRobotSync.WakeUpAll
def WakeUpAll(self)
Definition: car.py:66

py
Author(s):
autogenerated on Sat May 18 2019 02:32:55