m1013x2.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 void SigHandler(int sig)
26 {
27  // Do some custom action.
28  // For example, publish a stop message to some other nodes.
29 
30  // All the default sigint handler does is call shutdown()
31  ROS_INFO("shutdown time!");
32  ROS_INFO("shutdown time!");
33  ROS_INFO("shutdown time!");
34  //ros::ServiceClient srvMoveStop = nh.serviceClient<dsr_msgs::MoveStop>("/doosan_robot/motion/move_stop");
35 
36  ros::NodeHandlePtr node = boost::make_shared<ros::NodeHandle>();
37  //ros::Publisher pubRobotStop = node->advertise<dsr_msgs::RobotStop>("/doosan_robot/stop",100);
38  ros::Publisher pubRobotStop = node->advertise<dsr_msgs::RobotStop>("/"+ROBOT_ID +ROBOT_MODEL +"/stop",100);
39  ros::Publisher pubRobotStop2= node->advertise<dsr_msgs::RobotStop>("/"+ROBOT_ID2 +ROBOT_MODEL2+"/stop",100);
40 
41  dsr_msgs::RobotStop msg;
42 
43  msg.stop_mode = STOP_TYPE_QUICK;
44  pubRobotStop.publish(msg);
45 
46  ros::shutdown();
47 }
48 
49 int main(int argc, char** argv)
50 {
51  ros::init(argc, argv, "m1013x2_cpp", ros::init_options::NoSigintHandler);
52  ros::NodeHandle nh("~");
53  // Override the default ros sigint handler.
54  // This must be set after the first NodeHandle is created.
55  signal(SIGINT, SigHandler);
56 
57  ros::Publisher pubRobotStop = nh.advertise<dsr_msgs::RobotStop>("/"+ROBOT_ID +ROBOT_MODEL +"/stop",10);
58  ros::Publisher pubRobotStop2 = nh.advertise<dsr_msgs::RobotStop>("/"+ROBOT_ID2 +ROBOT_MODEL2+"/stop",10);
59 
60  //CDsrRobot robot(nh);
61  CDsrRobot r1(nh, ROBOT_ID, ROBOT_MODEL);
62  CDsrRobot r2(nh, ROBOT_ID2, ROBOT_MODEL2);
63 
65 
66  float JReady[6] = {0, -20, 110, 0, 60, 0}; //posj
67 
68  float TCP_POS[6] = {0, 0, 0,0 ,0, 0};
69  float J00[6] = {-180, 0, -145, 0, -35, 0};
70 
71  float J01r[6] = {-180.0, 71.4, -145.0, 0.0, -9.7, 0.0};
72  float J02r[6] = {-180.0, 67.7, -144.0, 0.0, 76.3, 0.0};
73  float J03r[6] = {-180.0, 0.0, 0.0, 0.0, 0.0, 0.0};
74 
75  float J04r[6] = {-90.0, 0.0, 0.0, 0.0, 0.0, 0.0};
76  float J04r1[6] = {-90.0, 30.0, -60.0, 0.0, 30.0, -0.0};
77  float J04r2[6] = {-90.0, -45.0, 90.0, 0.0, -45.0, -0.0};
78  float J04r3[6] = {-90.0, 60.0, -120.0, 0.0, 60.0, -0.0};
79  float J04r4[6] = {-90.0, 0.0, -0.0, 0.0, 0.0, -0.0};
80 
81  float J05r[6] = {-144.0, -4.0, -84.8, -90.9, 54.0, -1.1};
82  float J06r[6];
83  float J07r[6] = {-152.4, 12.4, -78.6, 18.7, -68.3, -37.7};
84  float J08r[6] = {-90.0, 30.0, -120.0, -90.0, -90.0, 0.0};
85 
86  float JEnd[6] = {0.0, -12.6, 101.1, 0.0, 91.5, -0.0};
87 
88  float dREL1[6] = {0, 0, 350, 0, 0, 0};
89  float dREL2[6] = {0, 0, -350, 0, 0, 0};
90 
91  float velx[2] = {0,0};
92  float accx[2] = {0, 0};
93 
94  float vel_spi[2] = {400, 400};
95  float acc_spi[2] = {150, 150};
96 
97  float J1[6] = {81.2, 20.8, 127.8, 162.5, 56.1, -37.1};
98  float X0[6] = {-88.7, 799.0, 182.3, 95.7, 93.7, 133.9};
99  float X1[6] = {304.2, 871.8, 141.5, 99.5, 84.9, 133.4};
100  float X2[6] = {437.1, 876.9, 362.1, 99.6, 84.0, 132.1};
101  float X3[6] = {-57.9, 782.4, 478.4, 99.6, 84.0, 132.1};
102 
103  float amp[6] = {0, 0, 0, 30, 30, 0};
104  float period[6] = {0, 0, 0, 3, 6, 0};
105 
106  float x01[6] = {423.6, 334.5, 651.2, 84.7, -180.0, 84.7};
107  float x02[6] = {423.6, 34.5, 951.2, 68.2, -180.0, 68.2};
108  float x03[6] = {423.6, -265.5, 651.2, 76.1, -180.0, 76.1};
109  float x04[6] = {423.6, 34.5, 351.2, 81.3, -180.0, 81.3};
110  float x0204c[2][6];
112 
113 
114  while(ros::ok())
115  {
116  r1.amovej(JReady, 20, 20);
117  r2.amovej(JReady, 20, 20);
118  r1.move_wait(); r2.move_wait();
119 
120  r1.config_create_tcp("TCP_ZERO", TCP_POS);
121  r2.config_create_tcp("TCP_ZERO", TCP_POS);
122  r1.set_current_tcp("TCP_ZERO");
123  r2.set_current_tcp("TCP_ZERO");
124 
125  r1.amovej(J1, 0, 0, 3);
126  r2.amovej(J1, 0, 0, 3);
127  r1.move_wait(); r2.move_wait();
128 
129  r1.amovel(X3, velx, accx, 2.5);
130  r2.amovel(X3, velx, accx, 2.5);
131  r1.move_wait(); r2.move_wait();
132 
133  for(int i=0; i<2; i++){
134  r1.amovel(X2, velx, accx, 2.5, MOVE_MODE_ABSOLUTE, MOVE_REFERENCE_BASE, 50);
135  r2.amovel(X2, velx, accx, 2.5, MOVE_MODE_ABSOLUTE, MOVE_REFERENCE_BASE, 50);
136  r1.move_wait(); r2.move_wait();
137 
138  r1.amovel(X1, velx, accx, 1.5, MOVE_MODE_ABSOLUTE, MOVE_REFERENCE_BASE, 50);
139  r2.amovel(X1, velx, accx, 1.5, MOVE_MODE_ABSOLUTE, MOVE_REFERENCE_BASE, 50);
140  r1.move_wait(); r2.move_wait();
141 
142  r1.amovel(X0, velx, accx, 2.5);
143  r2.amovel(X0, velx, accx, 2.5);
144  r1.move_wait(); r2.move_wait();
145 
146  r1.amovel(X1, velx, accx, 2.5, MOVE_MODE_ABSOLUTE, MOVE_REFERENCE_BASE, 50);
147  r2.amovel(X1, velx, accx, 2.5, MOVE_MODE_ABSOLUTE, MOVE_REFERENCE_BASE, 50);
148  r1.move_wait(); r2.move_wait();
149 
150  r1.amovel(X2, velx, accx, 1.5, MOVE_MODE_ABSOLUTE, MOVE_REFERENCE_BASE, 50);
151  r2.amovel(X2, velx, accx, 1.5, MOVE_MODE_ABSOLUTE, MOVE_REFERENCE_BASE, 50);
152  r1.move_wait(); r2.move_wait();
153 
154  r1.amovel(X3, velx, accx, 2.5, MOVE_MODE_ABSOLUTE, MOVE_REFERENCE_BASE, 50);
155  r2.amovel(X3, velx, accx, 2.5, MOVE_MODE_ABSOLUTE, MOVE_REFERENCE_BASE, 50);
156  r1.move_wait(); r2.move_wait();
157  }
158 
159  r1.amovej(J00, 60, 60, 6);
160  r2.amovej(J00, 60, 60, 6);
161  r1.move_wait(); r2.move_wait();
162 
163  r1.amovej(J01r, 0, 0, 2, MOVE_MODE_ABSOLUTE, 100);
164  r2.amovej(J01r, 0, 0, 2, MOVE_MODE_ABSOLUTE, 100);
165  r1.move_wait(); r2.move_wait();
166 
167  r1.amovej(J02r, 0, 0, 2, MOVE_MODE_ABSOLUTE, 50);
168  r2.amovej(J02r, 0, 0, 2, MOVE_MODE_ABSOLUTE, 50);
169  r1.move_wait(); r2.move_wait();
170 
171  r1.amovej(J03r, 0, 0, 2);
172  r2.amovej(J03r, 0, 0, 2);
173  r1.move_wait(); r2.move_wait();
174 
175  r1.amovej(J04r, 0, 0, 1.5);
176  r2.amovej(J04r, 0, 0, 1.5);
177  r1.move_wait(); r2.move_wait();
178 
179  r1.amovej(J04r1, 0, 0, 2, MOVE_MODE_ABSOLUTE, 50);
180  r2.amovej(J04r1, 0, 0, 2, MOVE_MODE_ABSOLUTE, 50);
181  r1.move_wait(); r2.move_wait();
182 
183  r1.amovej(J04r2, 0, 0, 4, MOVE_MODE_ABSOLUTE, 50);
184  r2.amovej(J04r2, 0, 0, 4, MOVE_MODE_ABSOLUTE, 50);
185  r1.move_wait(); r2.move_wait();
186 
187  r1.amovej(J04r3, 0, 0, 4, MOVE_MODE_ABSOLUTE, 50);
188  r2.amovej(J04r3, 0, 0, 4, MOVE_MODE_ABSOLUTE, 50);
189  r1.move_wait(); r2.move_wait();
190 
191  r1.amovej(J04r4, 0, 0, 2);
192  r2.amovej(J04r4, 0, 0, 2);
193  r1.move_wait(); r2.move_wait();
194 
195  r1.amovej(J05r, 0, 0, 2.5, MOVE_MODE_ABSOLUTE, 100);
196  r2.amovej(J05r, 0, 0, 2.5, MOVE_MODE_ABSOLUTE, 100);
197  r1.move_wait(); r2.move_wait();
198 
199  r1.amovel(dREL1, velx, accx, 1, MOVE_MODE_ABSOLUTE, MOVE_REFERENCE_TOOL, 50);
200  r2.amovel(dREL1, velx, accx, 1, MOVE_MODE_ABSOLUTE, MOVE_REFERENCE_TOOL, 50);
201  r1.move_wait(); r2.move_wait();
202 
203  r1.amovel(dREL2, velx, accx, 1.5, MOVE_MODE_ABSOLUTE, MOVE_REFERENCE_TOOL, 100);
204  r2.amovel(dREL2, velx, accx, 1.5, MOVE_MODE_ABSOLUTE, MOVE_REFERENCE_TOOL, 100);
205  r1.move_wait(); r2.move_wait();
206 
207  r1.amovej(J07r, 60, 60, 1.5, MOVE_MODE_ABSOLUTE, 100);
208  r2.amovej(J07r, 60, 60, 1.5, MOVE_MODE_ABSOLUTE, 100);
209  r1.move_wait(); r2.move_wait();
210 
211  r1.amovej(J08r, 60, 60, 2);
212  r2.amovej(J08r, 60, 60, 2);
213  r1.move_wait(); r2.move_wait();
214 
215  r1.amovej(JEnd, 60, 60, 4);
216  r2.amovej(JEnd, 60, 60, 4);
217  r1.move_wait(); r2.move_wait();
218 
219  r1.amove_periodic(amp, period, 0, 1, MOVE_REFERENCE_TOOL);
220  r2.amove_periodic(amp, period, 0, 1, MOVE_REFERENCE_TOOL);
221  r1.move_wait(); r2.move_wait();
222 
223  r1.amove_spiral(3, 200, 100, vel_spi, acc_spi, 0, TASK_AXIS_X, MOVE_REFERENCE_TOOL);
224  r2.amove_spiral(3, 200, 100, vel_spi, acc_spi, 0, TASK_AXIS_X, MOVE_REFERENCE_TOOL);
225  r1.move_wait(); r2.move_wait();
226 
227  r1.amovel(x01, velx, accx, 2);
228  r2.amovel(x01, velx, accx, 2);
229  r1.move_wait(); r2.move_wait();
230 
231  r1.amovel(x04, velx, accx, 2, MOVE_MODE_ABSOLUTE, MOVE_REFERENCE_BASE, 100);
232  r2.amovel(x04, velx, accx, 2, MOVE_MODE_ABSOLUTE, MOVE_REFERENCE_BASE, 100);
233  r1.move_wait(); r2.move_wait();
234 
235  r1.amovel(x03, velx, accx, 2, MOVE_MODE_ABSOLUTE, MOVE_REFERENCE_BASE, 100);
236  r2.amovel(x04, velx, accx, 2, MOVE_MODE_ABSOLUTE, MOVE_REFERENCE_BASE, 100);
237  r1.move_wait(); r2.move_wait();
238 
239  r1.amovel(x02, velx, accx, 2, MOVE_MODE_ABSOLUTE, MOVE_REFERENCE_BASE, 100);
240  r2.amovel(x02, velx, accx, 2, MOVE_MODE_ABSOLUTE, MOVE_REFERENCE_BASE, 100);
241  r1.move_wait(); r2.move_wait();
242 
243  r1.amovel(x01, velx, accx, 2);
244  r2.amovel(x01, velx, accx, 2);
245  r1.move_wait(); r2.move_wait();
246 
247  for(int i=0; i<6; i++){
248  x0204c[0][i] = x02[i];
249  x0204c[1][i] = x04[i];
250  }
251  r1.amovec(x0204c, velx, accx, 4 , MOVE_MODE_ABSOLUTE, MOVE_REFERENCE_BASE, 360);
252  r2.amovec(x0204c, velx, accx, 4 , MOVE_MODE_ABSOLUTE, MOVE_REFERENCE_BASE, 360);
253  r1.move_wait(); r2.move_wait();
254 
255  }
256 
257  ROS_INFO("dance_m1013 finished !!!!!!!!!!!!!!!!!!!!!");
258  return 0;
259 }
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
JReady
float JReady[6]
Definition: m1013x2_sync.cpp:31
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
JEnd
float JEnd[6]
Definition: m1013x2_sync2.cpp:49
DSR_Robot
J04r2
float J04r2[6]
Definition: m1013x2_sync2.cpp:42
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
ROBOT_MODEL
string ROBOT_MODEL
Definition: m1013x2.cpp:20
SigHandler
void SigHandler(int sig)
Definition: m1013x2.cpp:25
ROBOT_ID
string ROBOT_ID
Definition: m1013x2.cpp:19
ROS_INFO
#define ROS_INFO(...)
J1
float J1[6]
Definition: m1013x2_sync2.cpp:56
J01r
float J01r[6]
Definition: m1013x2_sync.cpp:34
dREL1
float dREL1[6]
Definition: m1013x2_sync2.cpp:50
X3
float X3[6]
Definition: m1013x2_sync2.cpp:60
ROBOT_ID2
string ROBOT_ID2
Definition: m1013x2.cpp:21
ROBOT_MODEL2
string ROBOT_MODEL2
Definition: m1013x2.cpp:22
ros::ok
ROSCPP_DECL bool ok()
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
main
int main(int argc, char **argv)
Definition: m1013x2.cpp:49
dREL2
float dREL2[6]
Definition: m1013x2_sync2.cpp:51
x02
float x02[6]
Definition: m1013x2_sync2.cpp:64
TCP_POS
float TCP_POS[6]
Definition: m1013x2_sync.cpp:32
J00
float J00[6]
Definition: m1013x2_sync.cpp:33
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
ros::shutdown
ROSCPP_DECL void shutdown()
J03r
float J03r[6]
Definition: m1013x2_sync2.cpp:39
J04r4
float J04r4[6]
Definition: m1013x2_sync2.cpp:44
J07r
float J07r[6]
Definition: m1013x2_sync2.cpp:47

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