13 #include "dsr_robot.h" 43 std::cout <<
">>>>>>>>>>>>>>>>>>>> send wake-up [R2]" << std::endl;
49 boost::this_thread::sleep(boost::posix_time::milliseconds(10));
55 std::cout <<
"<<<<<<<<<<<<<<<<<<<< wait signal form [R1]" << std::endl;
69 std::cout <<
">>>>>>>>>>>>>>>>>>>> send wake-up [R1]" << std::endl;
75 boost::this_thread::sleep(boost::posix_time::milliseconds(10));
81 std::cout <<
"<<<<<<<<<<<<<<<<<<<< wait signal form [R2]" << std::endl;
92 float JReady[6] = {0, -20, 110, 0, 60, 0};
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};
97 CDsrRobot r1(nh,
"dsr01",
"m1013");
100 r1.movej(JReady, 20, 20);
108 r1.movej(J00, 20, 20);
112 r1.movej(J01r, 20, 20);
123 float JReady[6] = {0, -20, 110, 0, 60, 0};
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};
128 CDsrRobot r2(nh,
"dsr02",
"m1013");
131 r2.movej(JReady, 20, 20);
139 r2.movej(J00, 20, 20);
143 r2.movej(J01r, 20, 20);
166 dsr_msgs::RobotStop msg;
168 msg.stop_mode = STOP_TYPE_QUICK;
170 pubRobotStop.publish(msg);
171 pubRobotStop2.publish(msg);
176 int main(
int argc,
char** argv)
202 CDsrRobot r1(nh,
"dsr01",
"m1013");
203 CDsrRobot r2(nh,
"dsr02",
"m1013");
207 float JReady[6] = {0, -20, 110, 0, 60, 0};
209 float TCP_POS[6] = {0, 0, 0,0 ,0, 0};
210 float J00[6] = {-180, 0, -145, 0, -35, 0};
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};
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};
222 float J05r[6] = {-144.0, -4.0, -84.8, -90.9, 54.0, -1.1};
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};
227 float JEnd[6] = {0.0, -12.6, 101.1, 0.0, 91.5, -0.0};
229 float dREL1[6] = {0, 0, 350, 0, 0, 0};
230 float dREL2[6] = {0, 0, -350, 0, 0, 0};
232 float velx[2] = {0,0};
233 float accx[2] = {0, 0};
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};
244 float amp[6] = {0, 0, 0, 30, 30, 0};
245 float period[6] = {0, 0, 0, 3, 6, 0};
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};
258 r1.amovej(JReady, 20, 20);
259 r2.amovej(JReady, 20, 20);
260 r1.move_wait(); r2.move_wait();
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");
267 r1.amovej(J1, 0, 0, 3);
268 r2.amovej(J1, 0, 0, 3);
269 r1.move_wait(); r2.move_wait();
271 r1.amovel(X3, velx, accx, 2.5);
272 r2.amovel(X3, velx, accx, 2.5);
273 r1.move_wait(); r2.move_wait();
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();
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();
284 r1.amovel(X0, velx, accx, 2.5);
285 r2.amovel(X0, velx, accx, 2.5);
286 r1.move_wait(); r2.move_wait();
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();
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();
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();
301 r1.amovej(J00, 60, 60, 6);
302 r2.amovej(J00, 60, 60, 6);
303 r1.move_wait(); r2.move_wait();
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();
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();
313 r1.amovej(J03r, 0, 0, 2);
314 r2.amovej(J03r, 0, 0, 2);
315 r1.move_wait(); r2.move_wait();
317 r1.amovej(J04r, 0, 0, 1.5);
318 r2.amovej(J04r, 0, 0, 1.5);
319 r1.move_wait(); r2.move_wait();
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();
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();
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();
333 r1.amovej(J04r4, 0, 0, 2);
334 r2.amovej(J04r4, 0, 0, 2);
335 r1.move_wait(); r2.move_wait();
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();
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();
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();
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();
353 r1.amovej(J08r, 60, 60, 2);
354 r2.amovej(J08r, 60, 60, 2);
355 r1.move_wait(); r2.move_wait();
357 r1.amovej(JEnd, 60, 60, 4);
358 r2.amovej(JEnd, 60, 60, 4);
359 r1.move_wait(); r2.move_wait();
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();
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();
369 r1.amovel(x01, velx, accx, 2);
370 r2.amovel(x01, velx, accx, 2);
371 r1.move_wait(); r2.move_wait();
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();
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();
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();
385 r1.amovel(x01, velx, accx, 2);
386 r2.amovel(x01, velx, accx, 2);
387 r1.move_wait(); r2.move_wait();
389 for(
int i=0; i<6; i++){
390 x0204c[0][i] = x02[i];
391 x0204c[1][i] = x04[i];
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();
400 ROS_INFO(
"m1013x2_sync_cpp finished !!!!!!!!!!!!!!!!!!!!!");
void notify_all() BOOST_NOEXCEPT
boost::condition_variable condition
void wait(unique_lock< mutex > &m)
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)
int main(int argc, char **argv)
void thread_robot2(ros::NodeHandle nh)
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
void thread_robot1(ros::NodeHandle nh)
boost::mutex::scoped_lock lock(io_mutex)
ROSCPP_DECL void shutdown()
void notify_one() BOOST_NOEXCEPT