m1013x2_sync2.cpp
Go to the documentation of this file.
1 /*
2  * [c++ demo] muliti robot sync test
3  * Author: Kab Kyoum Kim (kabkyoum.kim@doosan.com)
4  *
5  * Copyright (c) 2019 Doosan Robotics
6  * Use of this source code is governed by the BSD, see LICENSE
7 */
8 
9 #include <ros/ros.h>
10 #include <signal.h>
11 #include <time.h>
12 #include <chrono>
13 
14 #include "dsr_util.h"
15 #include "dsr_robot.h"
16 
17 using namespace std;
18 using namespace DSR_Util;
19 using namespace DSR_Robot;
20 
21 //----- set tartget robot----------------------------------------------------
22 #define NUM_ROBOT 2
23 string ROBOT_ID = "dsr01";
24 string ROBOT_MODEL = "m1013";
25 string ROBOT_ID2 = "dsr02";
26 string ROBOT_MODEL2 = "m1013";
27 
29 
30 CRobotSync RobotSync(NUM_ROBOT);
31 
32 
33 float JReady[6] = {0, -20, 110, 0, 60, 0}; //posj
34 
35 float TCP_POS[6] = {0, 0, 0,0 ,0, 0};
36 float J00[6] = {-180, 0, -145, 0, -35, 0};
37 float J01r[6] = {-180.0, 71.4, -145.0, 0.0, -9.7, 0.0};
38 float J02r[6] = {-180.0, 67.7, -144.0, 0.0, 76.3, 0.0};
39 float J03r[6] = {-180.0, 0.0, 0.0, 0.0, 0.0, 0.0};
40 float J04r[6] = {-90.0, 0.0, 0.0, 0.0, 0.0, 0.0};
41 float J04r1[6] = {-90.0, 30.0, -60.0, 0.0, 30.0, -0.0};
42 float J04r2[6] = {-90.0, -45.0, 90.0, 0.0, -45.0, -0.0};
43 float J04r3[6] = {-90.0, 60.0, -120.0, 0.0, 60.0, -0.0};
44 float J04r4[6] = {-90.0, 0.0, -0.0, 0.0, 0.0, -0.0};
45 float J05r[6] = {-144.0, -4.0, -84.8, -90.9, 54.0, -1.1};
46 float J06r[6];
47 float J07r[6] = {-152.4, 12.4, -78.6, 18.7, -68.3, -37.7};
48 float J08r[6] = {-90.0, 30.0, -120.0, -90.0, -90.0, 0.0};
49 float JEnd[6] = {0.0, -12.6, 101.1, 0.0, 91.5, -0.0};
50 float dREL1[6] = {0, 0, 350, 0, 0, 0};
51 float dREL2[6] = {0, 0, -350, 0, 0, 0};
52 float velx[2] = {0,0};
53 float accx[2] = {0, 0};
54 float vel_spi[2] = {400, 400};
55 float acc_spi[2] = {150, 150};
56 float J1[6] = {81.2, 20.8, 127.8, 162.5, 56.1, -37.1};
57 float X0[6] = {-88.7, 799.0, 182.3, 95.7, 93.7, 133.9};
58 float X1[6] = {304.2, 871.8, 141.5, 99.5, 84.9, 133.4};
59 float X2[6] = {437.1, 876.9, 362.1, 99.6, 84.0, 132.1};
60 float X3[6] = {-57.9, 782.4, 478.4, 99.6, 84.0, 132.1};
61 float amp[6] = {0, 0, 0, 30, 30, 0};
62 float period[6] = {0, 0, 0, 3, 6, 0};
63 float x01[6] = {423.6, 334.5, 651.2, 84.7, -180.0, 84.7};
64 float x02[6] = {423.6, 34.5, 951.2, 68.2, -180.0, 68.2};
65 float x03[6] = {423.6, -265.5, 651.2, 76.1, -180.0, 76.1};
66 float x04[6] = {423.6, 34.5, 351.2, 81.3, -180.0, 81.3};
67 float x0204c[2][6];
69 
70 void thread_robot1(ros::NodeHandle nh)
71 {
72  int nRobotID = 0;
73  CDsrRobot r(nh,"dsr01","m1013");
74 
75  while(ros::ok())
76  {
77  RobotSync.Wait(nRobotID);
78  r.movej(JReady, 20, 20);
79 
80  RobotSync.Wait(nRobotID);
81  r.config_create_tcp("TCP_ZERO", TCP_POS);
82 
83  RobotSync.Wait(nRobotID);
84  r.set_current_tcp("TCP_ZERO");
85 
86  RobotSync.Wait(nRobotID);
87  r.movej(J1, 20, 20, 0);
88 
89  RobotSync.Wait(nRobotID);
90  r.movel(X3, velx, accx, 2.5);
91 
92  for(int i=0; i<2; i++){
93  RobotSync.Wait(nRobotID);
94  r.movel(X2, velx, accx, 0, MOVE_MODE_ABSOLUTE, MOVE_REFERENCE_BASE);
95 
96  RobotSync.Wait(nRobotID);
97  r.movel(X1, velx, accx, 0, MOVE_MODE_ABSOLUTE, MOVE_REFERENCE_BASE);
98 
99  RobotSync.Wait(nRobotID);
100  r.movel(X0, velx, accx, 0);
101 
102  RobotSync.Wait(nRobotID);
103  r.movel(X1, velx, accx, 0, MOVE_MODE_ABSOLUTE, MOVE_REFERENCE_BASE);
104 
105  RobotSync.Wait(nRobotID);
106  r.movel(X2, velx, accx, 0, MOVE_MODE_ABSOLUTE, MOVE_REFERENCE_BASE);
107 
108  RobotSync.Wait(nRobotID);
109  r.movel(X3, velx, accx, 0, MOVE_MODE_ABSOLUTE, MOVE_REFERENCE_BASE);
110  }
111 
112  RobotSync.Wait(nRobotID);
113  r.movej(J00, 60, 60);
114 
115  RobotSync.Wait(nRobotID);
116  r.movej(J01r, 30, 30, 0, MOVE_MODE_ABSOLUTE);
117 
118  RobotSync.Wait(nRobotID);
119  r.movej(J02r, 30, 30, 0, MOVE_MODE_ABSOLUTE);
120 
121  RobotSync.Wait(nRobotID);
122  r.movej(J03r, 30, 30, 0);
123 
124  RobotSync.Wait(nRobotID);
125  r.movej(J04r, 30, 30, 0);
126 
127  RobotSync.Wait(nRobotID);
128  r.movej(J04r1, 30, 30, 0, MOVE_MODE_ABSOLUTE);
129 
130  RobotSync.Wait(nRobotID);
131  r.movej(J04r2, 30, 30, 0, MOVE_MODE_ABSOLUTE);
132 
133  RobotSync.Wait(nRobotID);
134  r.movej(J04r3, 30, 30, 0, MOVE_MODE_ABSOLUTE);
135 
136  RobotSync.Wait(nRobotID);
137  r.movej(J04r4, 30, 30, 0);
138 
139  RobotSync.Wait(nRobotID);
140  r.movej(J05r, 30, 30, 0, MOVE_MODE_ABSOLUTE);
141 
142  RobotSync.Wait(nRobotID);
143  r.movel(dREL1, velx, accx, 0, MOVE_MODE_ABSOLUTE, MOVE_REFERENCE_TOOL);
144 
145  RobotSync.Wait(nRobotID);
146  r.movel(dREL2, velx, accx, 0, MOVE_MODE_ABSOLUTE, MOVE_REFERENCE_TOOL);
147 
148  RobotSync.Wait(nRobotID);
149  r.movej(J07r, 60, 60, 0, MOVE_MODE_ABSOLUTE);
150 
151  RobotSync.Wait(nRobotID);
152  r.movej(J08r, 60, 60);
153 
154  RobotSync.Wait(nRobotID);
155  r.movej(JEnd, 60, 60);
156 
157  RobotSync.Wait(nRobotID);
158  r.move_periodic(amp, period, 0, 2, MOVE_REFERENCE_TOOL);
159 
160  RobotSync.Wait(nRobotID);
161  r.move_spiral(3, 200, 100, vel_spi, acc_spi, 0, TASK_AXIS_X, MOVE_REFERENCE_TOOL);
162 
163  RobotSync.Wait(nRobotID);
164  r.movel(x01, velx, accx);
165 
166  RobotSync.Wait(nRobotID);
167  r.movel(x04, velx, accx, 0, MOVE_MODE_ABSOLUTE, MOVE_REFERENCE_BASE);
168 
169  RobotSync.Wait(nRobotID);
170  r.movel(x03, velx, accx, 0, MOVE_MODE_ABSOLUTE, MOVE_REFERENCE_BASE);
171 
172  RobotSync.Wait(nRobotID);
173  r.movel(x02, velx, accx, 0, MOVE_MODE_ABSOLUTE, MOVE_REFERENCE_BASE);
174 
175  RobotSync.Wait(nRobotID);
176  r.movel(x01, velx, accx);
177 
178  for(int i=0; i<6; i++){
179  x0204c[0][i] = x02[i];
180  x0204c[1][i] = x04[i];
181  }
182 
183  RobotSync.Wait(nRobotID);
184  r.movec(x0204c, velx, accx, 0 , MOVE_MODE_ABSOLUTE, MOVE_REFERENCE_BASE);
185  }
186 }
187 
188 void thread_robot2(ros::NodeHandle nh)
189 {
190  int nRobotID = 1;
191  CDsrRobot r(nh,"dsr02","m1013");
192 
193  while(ros::ok())
194  {
195  RobotSync.Wait(nRobotID);
196  r.movej(JReady, 20, 20);
197 
198  RobotSync.Wait(nRobotID);
199  r.config_create_tcp("TCP_ZERO", TCP_POS);
200 
201  RobotSync.Wait(nRobotID);
202  r.set_current_tcp("TCP_ZERO");
203 
204  RobotSync.Wait(nRobotID);
205  r.movej(J1, 20, 20, 0);
206 
207  RobotSync.Wait(nRobotID);
208  r.movel(X3, velx, accx, 2.5);
209 
210  for(int i=0; i<2; i++){
211  RobotSync.Wait(nRobotID);
212  r.movel(X2, velx, accx, 0, MOVE_MODE_ABSOLUTE, MOVE_REFERENCE_BASE);
213 
214  RobotSync.Wait(nRobotID);
215  r.movel(X1, velx, accx, 0, MOVE_MODE_ABSOLUTE, MOVE_REFERENCE_BASE);
216 
217  RobotSync.Wait(nRobotID);
218  r.movel(X0, velx, accx, 0);
219 
220  RobotSync.Wait(nRobotID);
221  r.movel(X1, velx, accx, 0, MOVE_MODE_ABSOLUTE, MOVE_REFERENCE_BASE);
222 
223  RobotSync.Wait(nRobotID);
224  r.movel(X2, velx, accx, 0, MOVE_MODE_ABSOLUTE, MOVE_REFERENCE_BASE);
225 
226  RobotSync.Wait(nRobotID);
227  r.movel(X3, velx, accx, 0, MOVE_MODE_ABSOLUTE, MOVE_REFERENCE_BASE);
228  }
229 
230  RobotSync.Wait(nRobotID);
231  r.movej(J00, 60, 60);
232 
233  RobotSync.Wait(nRobotID);
234  r.movej(J01r, 30, 30, 0, MOVE_MODE_ABSOLUTE);
235 
236  RobotSync.Wait(nRobotID);
237  r.movej(J02r, 30, 30, 0, MOVE_MODE_ABSOLUTE);
238 
239  RobotSync.Wait(nRobotID);
240  r.movej(J03r, 30, 30, 0);
241 
242  RobotSync.Wait(nRobotID);
243  r.movej(J04r, 30, 30, 0);
244 
245  RobotSync.Wait(nRobotID);
246  r.movej(J04r1, 30, 30, 0, MOVE_MODE_ABSOLUTE);
247 
248  RobotSync.Wait(nRobotID);
249  r.movej(J04r2, 30, 30, 0, MOVE_MODE_ABSOLUTE);
250 
251  RobotSync.Wait(nRobotID);
252  r.movej(J04r3, 30, 30, 0, MOVE_MODE_ABSOLUTE);
253 
254  RobotSync.Wait(nRobotID);
255  r.movej(J04r4, 30, 30, 0);
256 
257  RobotSync.Wait(nRobotID);
258  r.movej(J05r, 30, 30, 0, MOVE_MODE_ABSOLUTE);
259 
260  RobotSync.Wait(nRobotID);
261  r.movel(dREL1, velx, accx, 0, MOVE_MODE_ABSOLUTE, MOVE_REFERENCE_TOOL);
262 
263  RobotSync.Wait(nRobotID);
264  r.movel(dREL2, velx, accx, 0, MOVE_MODE_ABSOLUTE, MOVE_REFERENCE_TOOL);
265 
266  RobotSync.Wait(nRobotID);
267  r.movej(J07r, 60, 60, 0, MOVE_MODE_ABSOLUTE);
268 
269  RobotSync.Wait(nRobotID);
270  r.movej(J08r, 60, 60);
271 
272  RobotSync.Wait(nRobotID);
273  r.movej(JEnd, 60, 60);
274 
275  RobotSync.Wait(nRobotID);
276  r.move_periodic(amp, period, 0, 2, MOVE_REFERENCE_TOOL);
277 
278  RobotSync.Wait(nRobotID);
279  r.move_spiral(3, 200, 100, vel_spi, acc_spi, 0, TASK_AXIS_X, MOVE_REFERENCE_TOOL);
280 
281  RobotSync.Wait(nRobotID);
282  r.movel(x01, velx, accx);
283 
284  RobotSync.Wait(nRobotID);
285  r.movel(x04, velx, accx, 0, MOVE_MODE_ABSOLUTE, MOVE_REFERENCE_BASE);
286 
287  RobotSync.Wait(nRobotID);
288  r.movel(x03, velx, accx, 0, MOVE_MODE_ABSOLUTE, MOVE_REFERENCE_BASE);
289 
290  RobotSync.Wait(nRobotID);
291  r.movel(x02, velx, accx, 0, MOVE_MODE_ABSOLUTE, MOVE_REFERENCE_BASE);
292 
293  RobotSync.Wait(nRobotID);
294  r.movel(x01, velx, accx);
295 
296  for(int i=0; i<6; i++){
297  x0204c[0][i] = x02[i];
298  x0204c[1][i] = x04[i];
299  }
300 
301  RobotSync.Wait(nRobotID);
302  r.movec(x0204c, velx, accx, 0 , MOVE_MODE_ABSOLUTE, MOVE_REFERENCE_BASE);
303  }
304 }
305 
306 void thread_robot3(ros::NodeHandle nh)
307 {
308  int nRobotID = 2;
309  while(ros::ok())
310  {
311  //ROS_INFO("thread_robot3 running...");
312  RobotSync.Wait(nRobotID);
313  ROS_INFO("R3 do somethind...");
314  time_sleep(0.1);
316  }
317 }
318 
319 void thread_robot4(ros::NodeHandle nh)
320 {
321  int nRobotID = 3;
322  while(ros::ok())
323  {
324  //ROS_INFO("thread_robot4 running...");
325  RobotSync.Wait(nRobotID);
326  ROS_INFO("R4 do somethind...");
327  time_sleep(0.1);
329  }
330 }
331 
332 void thread_robot5(ros::NodeHandle nh)
333 {
334  int nRobotID = 4;
335  while(ros::ok())
336  {
337  //ROS_INFO("thread_robot5 running...");
338  RobotSync.Wait(nRobotID);
339  //ROS_INFO("R5 do somethind...");
340  time_sleep(0.1);
342  }
343 }
344 
345 void thread_robot6(ros::NodeHandle nh)
346 {
347  int nRobotID = 5;
348  while(1)
349  {
350  //ROS_INFO("thread_robot6 running...");
351  RobotSync.Wait(nRobotID);
352  //ROS_INFO("R6 do somethind...");
353  time_sleep(0.1);
354  }
355 }
356 
357 void SigHandler(int sig)
358 {
359  // Do some custom action.
360  // For example, publish a stop message to some other nodes.
361 
362  // All the default sigint handler does is call shutdown()
363  ROS_INFO("shutdown time!");
364  ROS_INFO("shutdown time!");
365  ROS_INFO("shutdown time!");
366  //ros::ServiceClient srvMoveStop = nh.serviceClient<dsr_msgs::MoveStop>("/doosan_robot/motion/move_stop");
367 
368  ros::NodeHandlePtr node = boost::make_shared<ros::NodeHandle>();
369  //ros::Publisher pubRobotStop = node->advertise<dsr_msgs::RobotStop>("/doosan_robot/stop",100);
370  ros::Publisher pubRobotStop = node->advertise<dsr_msgs::RobotStop>("/"+ROBOT_ID +ROBOT_MODEL +"/stop",100);
371  ros::Publisher pubRobotStop2= node->advertise<dsr_msgs::RobotStop>("/"+ROBOT_ID2 +ROBOT_MODEL2+"/stop",100);
372 
373  dsr_msgs::RobotStop msg;
374 
375  msg.stop_mode = STOP_TYPE_QUICK;
376 
377  pubRobotStop.publish(msg);
378  pubRobotStop2.publish(msg);
379 
381  ros::shutdown();
382 }
383 
384 int main(int argc, char** argv)
385 {
386  ros::init(argc, argv, "m1013x2_sync2_cpp", ros::init_options::NoSigintHandler);
387  ros::NodeHandle nh("~");
388  // Override the default ros sigint handler.
389  // This must be set after the first NodeHandle is created.
390  signal(SIGINT, SigHandler);
391 
392  ros::Publisher pubRobotStop = nh.advertise<dsr_msgs::RobotStop>("/"+ROBOT_ID +ROBOT_MODEL +"/stop",10);
393  ros::Publisher pubRobotStop2 = nh.advertise<dsr_msgs::RobotStop>("/"+ROBOT_ID2 +ROBOT_MODEL2+"/stop",10);
394 
395  // spawn another thread
396  boost::thread thread_r1(thread_robot1, nh);
397  boost::thread thread_r2(thread_robot2, nh);
398  //boost::thread thread_r3(thread_robot3, nh);
399  //boost::thread thread_r4(thread_robot4, nh);
400  //boost::thread thread_r5(thread_robot5, nh);
401  //boost::thread thread_r6(thread_robot6, nh);
402 
403  time_sleep(1);
404 
405  double max_time = 0.0;
406  double cur_time = 0.0;
407 
408  while(ros::ok())
409  {
410  time_sleep(0.01);
411  RobotSync.WakeUpAll();
412  }
413 
414  ROS_INFO("m1013x2_sync2_cpp finished !!!!!!!!!!!!!!!!!!!!!");
415  return 0;
416 }
thread_robot1
void thread_robot1(ros::NodeHandle nh)
Definition: m1013x2_sync2.cpp:70
ros::NodeHandle
x0204c
float x0204c[2][6]
Definition: m1013x2_sync2.cpp:67
amp
float amp[6]
Definition: m1013x2_sync2.cpp:61
ros::init_options::NoSigintHandler
NoSigintHandler
velx
float velx[2]
Definition: m1013x2_sync2.cpp:52
J04r1
float J04r1[6]
Definition: m1013x2_sync2.cpp:41
x03
float x03[6]
Definition: m1013x2_sync2.cpp:65
J05r
float J05r[6]
Definition: m1013x2_sync2.cpp:45
thread_robot5
void thread_robot5(ros::NodeHandle nh)
Definition: m1013x2_sync2.cpp:332
NUM_ROBOT
#define NUM_ROBOT
Definition: m1013x2_sync2.cpp:22
ros::init
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
X0
float X0[6]
Definition: m1013x2_sync2.cpp:57
period
float period[6]
Definition: m1013x2_sync2.cpp:62
SigHandler
void SigHandler(int sig)
Definition: m1013x2_sync2.cpp:357
JEnd
float JEnd[6]
Definition: m1013x2_sync2.cpp:49
DSR_Robot
J04r2
float J04r2[6]
Definition: m1013x2_sync2.cpp:42
J00
float J00[6]
Definition: m1013x2_sync2.cpp:36
boost::shared_ptr< NodeHandle >
J02r
float J02r[6]
Definition: m1013x2_sync2.cpp:38
accx
float accx[2]
Definition: m1013x2_sync2.cpp:53
J04r
float J04r[6]
Definition: m1013x2_sync2.cpp:40
J06r
float J06r[6]
Definition: m1013x2_sync2.cpp:46
thread_robot6
void thread_robot6(ros::NodeHandle nh)
Definition: m1013x2_sync2.cpp:345
ROS_INFO
#define ROS_INFO(...)
J1
float J1[6]
Definition: m1013x2_sync2.cpp:56
dREL1
float dREL1[6]
Definition: m1013x2_sync2.cpp:50
X3
float X3[6]
Definition: m1013x2_sync2.cpp:60
ROBOT_MODEL
string ROBOT_MODEL
Definition: m1013x2_sync2.cpp:24
ros::ok
ROSCPP_DECL bool ok()
ROBOT_MODEL2
string ROBOT_MODEL2
Definition: m1013x2_sync2.cpp:26
x01
float x01[6]
Definition: m1013x2_sync2.cpp:63
vel_spi
float vel_spi[2]
Definition: m1013x2_sync2.cpp:54
x04
float x04[6]
Definition: m1013x2_sync2.cpp:66
ros::NodeHandle::advertise
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
J08r
float J08r[6]
Definition: m1013x2_sync2.cpp:48
acc_spi
float acc_spi[2]
Definition: m1013x2_sync2.cpp:55
dREL2
float dREL2[6]
Definition: m1013x2_sync2.cpp:51
ROBOT_ID2
string ROBOT_ID2
Definition: m1013x2_sync2.cpp:25
x02
float x02[6]
Definition: m1013x2_sync2.cpp:64
TCP_POS
float TCP_POS[6]
Definition: m1013x2_sync2.cpp:35
J01r
float J01r[6]
Definition: m1013x2_sync2.cpp:37
DSR_Util
JReady
float JReady[6]
Definition: m1013x2_sync2.cpp:33
ROBOT_ID
string ROBOT_ID
Definition: m1013x2_sync2.cpp:23
thread_robot3
void thread_robot3(ros::NodeHandle nh)
Definition: m1013x2_sync2.cpp:306
J04r3
float J04r3[6]
Definition: m1013x2_sync2.cpp:43
X2
float X2[6]
Definition: m1013x2_sync2.cpp:59
X1
float X1[6]
Definition: m1013x2_sync2.cpp:58
thread_robot4
void thread_robot4(ros::NodeHandle nh)
Definition: m1013x2_sync2.cpp:319
ros::shutdown
ROSCPP_DECL void shutdown()
J03r
float J03r[6]
Definition: m1013x2_sync2.cpp:39
J04r4
float J04r4[6]
Definition: m1013x2_sync2.cpp:44
thread_robot2
void thread_robot2(ros::NodeHandle nh)
Definition: m1013x2_sync2.cpp:188
main
int main(int argc, char **argv)
Definition: m1013x2_sync2.cpp:384
J07r
float J07r[6]
Definition: m1013x2_sync2.cpp:47
RobotSync
CRobotSync RobotSync(NUM_ROBOT)

cpp
Author(s): Kab Kyoum Kim , Jin Hyuk Gong , Jeongwoo Lee
autogenerated on Sat May 18 2019 02:32:53