m1013x2.py
Go to the documentation of this file.
1 #!/usr/bin/env python
2 # -*- coding: utf-8 -*-
3 # ##
4 # @brief [py demo] muliti robot 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 # for single robot
15 #import DR_init
16 #DR_init.__dsr__id = "dsr01"
17 #DR_init.__dsr__model = "m1013"
18 #from DSR_ROBOT import *
19 
20 # for mulit robot
21 ########from DSR_ROBOT_MULTI import *
22 from DSR_ROBOT import *
23 
24 def shutdown():
25  print "shutdown time!"
26  print "shutdown time!"
27  print "shutdown time!"
28 
29  pub_stop_r1.publish(stop_mode=STOP_TYPE_QUICK)
30  pub_stop_r2.publish(stop_mode=STOP_TYPE_QUICK)
31  return 0
32 
33 def msgRobotState_cb_r1(msg):
34  msgRobotState_cb_r1.count += 1
35 
36  if (0==(msgRobotState_cb_r1.count % 100)):
37  rospy.loginfo("________ ROBOT[1] STATUS ________")
38  print(" robot_state : %d" % (msg.robot_state))
39  print(" robot_state_str : %s" % (msg.robot_state_str))
40  print(" current_posj : %7.3f %7.3f %7.3f %7.3f %7.3f %7.3f" % (msg.current_posj[0],msg.current_posj[1],msg.current_posj[2],msg.current_posj[3],msg.current_posj[4],msg.current_posj[5]))
41 
42  #print(" io_control_box : %d" % (msg.io_control_box))
43  ##print(" io_modbus : %d" % (msg.io_modbus))
44  ##print(" error : %d" % (msg.error))
45  #print(" access_control : %d" % (msg.access_control))
46  #print(" homming_completed : %d" % (msg.homming_completed))
47  #print(" tp_initialized : %d" % (msg.tp_initialized))
48  print(" speed : %d" % (msg.speed))
49  #print(" mastering_need : %d" % (msg.mastering_need))
50  #print(" drl_stopped : %d" % (msg.drl_stopped))
51  #print(" disconnected : %d" % (msg.disconnected))
52 msgRobotState_cb_r1.count = 0
53 
54 def msgRobotState_cb_r2(msg):
55  msgRobotState_cb_r2.count += 1
56 
57  if (0==(msgRobotState_cb_r2.count % 100)):
58  rospy.loginfo("________ ROBOT[2] STATUS ________")
59  print(" robot_state : %d" % (msg.robot_state))
60  print(" robot_state_str : %s" % (msg.robot_state_str))
61  print(" current_posj : %7.3f %7.3f %7.3f %7.3f %7.3f %7.3f" % (msg.current_posj[0],msg.current_posj[1],msg.current_posj[2],msg.current_posj[3],msg.current_posj[4],msg.current_posj[5]))
62 
63  #print(" io_control_box : %d" % (msg.io_control_box))
64  ##print(" io_modbus : %d" % (msg.io_modbus))
65  ##print(" error : %d" % (msg.error))
66  #print(" access_control : %d" % (msg.access_control))
67  #print(" homming_completed : %d" % (msg.homming_completed))
68  #print(" tp_initialized : %d" % (msg.tp_initialized))
69  print(" speed : %d" % (msg.speed))
70  #print(" mastering_need : %d" % (msg.mastering_need))
71  #print(" drl_stopped : %d" % (msg.drl_stopped))
72  #print(" disconnected : %d" % (msg.disconnected))
73 msgRobotState_cb_r2.count = 0
74 
75 def thread_subscriber_r1(robot_id, robot_model):
76  rospy.Subscriber('/'+ robot_id + robot_model +'/state', RobotState, msgRobotState_cb_r1)
77  rospy.spin()
78  #rospy.spinner(2)
79 
80 def thread_subscriber_r2(robot_id, robot_model):
81  rospy.Subscriber('/'+ robot_id + robot_model +'/state', RobotState, msgRobotState_cb_r2)
82  rospy.spin()
83  #rospy.spinner(2)
84 
85 if __name__ == "__main__":
86  rospy.init_node('m1013x2_amove_py')
87  rospy.on_shutdown(shutdown)
88 
89  robot_id1 = "dsr01"; robot_model1 = "m1013"
90  robot_id2 = "dsr02"; robot_model2 = "m1013"
91 
92  r1 = CDsrRobot("dsr01","m1013")
93  r2 = CDsrRobot("dsr02","m1013")
94 
95  pub_stop_r1 = rospy.Publisher('/'+ robot_id1 + robot_model1 +'/stop', RobotStop, queue_size=10)
96  pub_stop_r2 = rospy.Publisher('/'+ robot_id2 + robot_model2 +'/stop', RobotStop, queue_size=10)
97 
98  t1 = threading.Thread(target=thread_subscriber_r1, args=(robot_id1, robot_model1))
99  t1.daemon = True
100  t1.start()
101 
102  t2 = threading.Thread(target=thread_subscriber_r2, args=(robot_id2, robot_model2))
103  t2.daemon = True
104  t2.start()
105 
106  #----------------------------------------------------------------------
107  JReady = posj(0, -20, 110, 0, 60, 0)
108 
109  J00 = posj(-180, 0, -145, 0, -35, 0)
110 
111 
112  J01r = posj(-180.0, 71.4, -145.0, 0.0, -9.7, 0.0)
113  J02r = posj(-180.0, 67.7, -144.0, 0.0, 76.3, 0.0)
114  J03r = posj(-180.0, 0.0, 0.0, 0.0, 0.0, 0.0)
115 
116  J04r = posj(-90.0, 0.0, 0.0, 0.0, 0.0, 0.0)
117  J04r1 = posj(-90.0, 30.0, -60.0, 0.0, 30.0, -0.0)
118  J04r2 = posj(-90.0, -45.0, 90.0, 0.0, -45.0, -0.0)
119  J04r3 = posj(-90.0, 60.0, -120.0, 0.0, 60.0, -0.0)
120  J04r4 = posj(-90.0, 0.0, -0.0, 0.0, 0.0, -0.0)
121 
122  J05r = posj(-144.0, -4.0, -84.8, -90.9, 54.0, -1.1)
123 
124  J07r = posj(-152.4, 12.4, -78.6, 18.7, -68.3, -37.7)
125  J08r = posj(-90.0, 30.0, -120.0, -90.0, -90.0, 0.0)
126 
127  JEnd = posj(0.0, -12.6, 101.1, 0.0, 91.5, -0.0)
128 
129  dREL1 = posx(0, 0, 350, 0, 0, 0)
130  dREL2 = posx(0, 0, -350, 0, 0, 0)
131 
132  velx = [0, 0]
133  accx = [0, 0]
134 
135  vel_spi = [400, 400]
136  acc_spi = [150, 150]
137 
138  J1 = posj(81.2, 20.8, 127.8, 162.5, 56.1, -37.1)
139  X0 = posx(-88.7, 799.0, 182.3, 95.7, 93.7, 133.9)
140  X1 = posx(304.2, 871.8, 141.5, 99.5, 84.9, 133.4)
141  X2 = posx(437.1, 876.9, 362.1, 99.6, 84.0, 132.1)
142  X3 = posx(-57.9, 782.4, 478.4, 99.6, 84.0, 132.1)
143 
144  amp = [0, 0, 0, 30, 30, 0]
145  period = [0, 0, 0, 3, 6, 0]
146 
147  x01 = [423.6, 334.5, 651.2, 84.7, -180.0, 84.7]
148  x02 = [423.6, 34.5, 951.2, 68.2, -180.0, 68.2]
149  x03 = [423.6, -265.5, 651.2, 76.1, -180.0, 76.1]
150  x04 = [423.6, 34.5, 351.2, 81.3, -180.0, 81.3]
151 
152  while not rospy.is_shutdown():
153  r1.amovej(JReady, 20, 20)
154  r2.amovej(JReady, 20, 20)
155  r1.mwait(); r2.mwait()
156 
157  r1.amovej(J1, time = 3)
158  r2.amovej(J1, time = 3)
159  r1.mwait(); r2.mwait()
160 
161  r1.amovel(X3, time = 2.5)
162  r2.amovel(X3, time = 2.5)
163  r1.mwait(); r2.mwait()
164 
165  for i in range(1, 3):
166  r1.amovel(X2, time = 2.5, r = 50)
167  r2.amovel(X2, time = 2.5, r = 50)
168  r1.mwait(); r2.mwait()
169 
170  r1.amovel(X1, time = 1.5, r = 50)
171  r2.amovel(X1, time = 1.5, r = 50)
172  r1.move_wait(); r2.move_wait()
173 
174  r1.amovel(X0, time = 2.5)
175  r2.amovel(X0, time = 2.5)
176  r1.move_wait(); r2.move_wait()
177 
178  r1.amovel(X1, time = 2.5, r = 50)
179  r2.amovel(X1, time = 2.5, r = 50)
180  r1.move_wait(); r2.move_wait()
181 
182  r1.amovel(X2, time = 1.5, r = 50)
183  r2.amovel(X2, time = 1.5, r = 50)
184  r1.move_wait(); r2.move_wait()
185 
186  r1.amovel(X3, time = 2.5, r = 50)
187  r2.amovel(X3, time = 2.5, r = 50)
188  r1.move_wait(); r2.move_wait()
189 
190  r1.amovej(J00, time = 6)
191  r2.amovej(J00, time = 6)
192  r1.move_wait(); r2.move_wait()
193 
194  r1.amovej(J01r, time = 2, r = 100)
195  r2.amovej(J01r, time = 2, r = 100)
196  r1.move_wait(); r2.move_wait()
197 
198  r1.amovej(J02r, time = 2, r = 50)
199  r2.amovej(J02r, time = 2, r = 50)
200  r1.move_wait(); r2.move_wait()
201 
202  r1.amovej(J03r, time = 2)
203  r2.amovej(J03r, time = 2)
204  r1.move_wait(); r2.move_wait()
205 
206  r1.amovej(J04r, time = 1.5)
207  r2.amovej(J04r, time = 1.5)
208  r1.move_wait(); r2.move_wait()
209 
210  r1.amovej(J04r1, time = 2, r = 50)
211  r2.amovej(J04r1, time = 2, r = 50)
212  r1.move_wait(); r2.move_wait()
213 
214  r1.amovej(J04r2, time = 4, r = 50)
215  r2.amovej(J04r2, time = 4, r = 50)
216  r1.move_wait(); r2.move_wait()
217 
218  r1.amovej(J04r3, time = 4, r = 50)
219  r2.amovej(J04r3, time = 4, r = 50)
220  r1.move_wait(); r2.move_wait()
221 
222  r1.amovej(J04r4, time = 2)
223  r2.amovej(J04r4, time = 2)
224  r1.move_wait(); r2.move_wait()
225 
226  r1.amovej(J05r, time = 2.5, r = 100)
227  r2.amovej(J05r, time = 2.5, r = 100)
228  r1.move_wait(); r2.move_wait()
229 
230  r1.amovel(dREL1, time = 1, ref = DR_TOOL, r = 50)
231  r2.amovel(dREL1, time = 1, ref = DR_TOOL, r = 50)
232  r1.move_wait(); r2.move_wait()
233 
234  r1.amovel(dREL2, time = 1.5, ref = DR_TOOL, r = 100)
235  r2.amovel(dREL2, time = 1.5, ref = DR_TOOL, r = 100)
236  r1.move_wait(); r2.move_wait()
237 
238  r1.amovej(J07r, time = 1.5, r = 100)
239  r2.amovej(J07r, time = 1.5, r = 100)
240  r1.move_wait(); r2.move_wait()
241 
242  r1.amovej(J08r, time = 2)
243  r2.amovej(J08r, time = 2)
244  r1.move_wait(); r2.move_wait()
245 
246  r1.amovej(JEnd, time = 4)
247  r2.amovej(JEnd, time = 4)
248  r1.move_wait(); r2.move_wait()
249 
250  r1.amove_periodic([0,0,0,30,30,0],[0,0,0,3,6,0], atime=0,repeat=1, ref=DR_TOOL)
251  r2.amove_periodic([0,0,0,30,30,0],[0,0,0,3,6,0], atime=0,repeat=1, ref=DR_TOOL)
252  r1.move_wait(); r2.move_wait()
253 
254  r1.amove_spiral (rev=3, rmax=200, lmax=100, vel=400, acc=150, axis=DR_AXIS_X, ref=DR_TOOL)
255  r2.amove_spiral (rev=3, rmax=200, lmax=100, vel=400, acc=150, axis=DR_AXIS_X, ref=DR_TOOL)
256  r1.move_wait(); r2.move_wait()
257 
258  r1.amovel(x01, time = 2)
259  r2.amovel(x01, time = 2)
260  r1.move_wait(); r2.move_wait()
261 
262  r1.amovel(x04, time = 2, r = 100)
263  r2.amovel(x04, time = 2, r = 100)
264  r1.move_wait(); r2.move_wait()
265 
266  r1.amovel(x03, time = 2, r = 100)
267  r2.amovel(x03, time = 2, r = 100)
268  r1.move_wait(); r2.move_wait()
269 
270  r1.amovel(x02, time = 2, r = 100)
271  r2.amovel(x02, time = 2, r = 100)
272  r1.move_wait(); r2.move_wait()
273 
274  r1.amovel(x01, time = 2)
275  r2.amovel(x01, time = 2)
276  r1.move_wait(); r2.move_wait()
277 
278  r1.amovec(x02, x04, time = 4, angle = 360)
279  r2.amovec(x02, x04, time = 4, angle = 360)
280  r1.move_wait(); r2.move_wait()
281  #----------------------------------------------------------------------
282 
283  print 'good bye!'
m1013x2.msgRobotState_cb_r1
def msgRobotState_cb_r1(msg)
Definition: m1013x2.py:33
m1013x2.msgRobotState_cb_r2
def msgRobotState_cb_r2(msg)
Definition: m1013x2.py:54
m1013x2.thread_subscriber_r1
def thread_subscriber_r1(robot_id, robot_model)
Definition: m1013x2.py:75
m1013x2.thread_subscriber_r2
def thread_subscriber_r2(robot_id, robot_model)
Definition: m1013x2.py:80
m1013x2.shutdown
def shutdown()
Definition: m1013x2.py:24

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