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

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