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


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