dsr_service_motion_basic.cpp
Go to the documentation of this file.
1 /*
2  * [c++ example basic] motion basic test for doosan robot
3  * Author: Jin Hyuk Gong (jinhyuk.gong@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 <boost/thread/thread.hpp>
12 #include <cstdlib>
13 #include <array>
14 #include <vector>
15 #include <algorithm>
16 #include <string>
17 #include <iostream>
18 
19 #include <dsr_msgs/RobotState.h>
20 #include <dsr_msgs/RobotStop.h>
21 
22 #include <dsr_msgs/MoveJoint.h>
23 #include <dsr_msgs/MoveLine.h>
24 #include <dsr_msgs/MoveJointx.h>
25 #include <dsr_msgs/MoveCircle.h>
26 #include <dsr_msgs/MoveSplineJoint.h>
27 #include <dsr_msgs/MoveSplineTask.h>
28 #include <dsr_msgs/MoveBlending.h>
29 #include <dsr_msgs/MoveSpiral.h>
30 #include <dsr_msgs/MovePeriodic.h>
31 #include <dsr_msgs/MoveWait.h>
32 
33 #include <dsr_msgs/ConfigCreateTcp.h>
34 #include <dsr_msgs/ConfigDeleteTcp.h>
35 #include <dsr_msgs/GetCurrentTcp.h>
36 #include <dsr_msgs/SetCurrentTcp.h>
37 
38 #include <dsr_msgs/SetCurrentTool.h>
39 #include <dsr_msgs/GetCurrentTool.h>
40 #include <dsr_msgs/ConfigCreateTool.h>
41 #include <dsr_msgs/ConfigDeleteTool.h>
42 
43 #include <dsr_msgs/SetCtrlBoxDigitalOutput.h>
44 #include <dsr_msgs/GetCtrlBoxDigitalInput.h>
45 #include <dsr_msgs/SetToolDigitalOutput.h>
46 #include <dsr_msgs/GetToolDigitalInput.h>
47 #include <dsr_msgs/SetCtrlBoxAnalogOutput.h>
48 #include <dsr_msgs/GetCtrlBoxAnalogInput.h>
49 #include <dsr_msgs/SetCtrlBoxAnalogOutputType.h>
50 #include <dsr_msgs/SetCtrlBoxAnalogInputType.h>
51 
52 #include <dsr_msgs/SetModbusOutput.h>
53 #include <dsr_msgs/GetModbusInput.h>
54 #include <dsr_msgs/ConfigCreateModbus.h>
55 #include <dsr_msgs/ConfigDeleteModbus.h>
56 
57 #include <dsr_msgs/DrlPause.h>
58 #include <dsr_msgs/DrlStart.h>
59 #include <dsr_msgs/DrlStop.h>
60 #include <dsr_msgs/DrlResume.h>
61 
62 #include "DRFL.h"
63 #include "DRFC.h"
64 #include "DRFS.h"
65 
66 using namespace std;
67 
68 //----- set tartget robot----------------------------------------------------
69 string ROBOT_ID = "dsr01";
70 string ROBOT_MODEL = "m1013";
71 void SET_ROBOT(string id, string model) {ROBOT_ID = id; ROBOT_MODEL= model;}
72 //---------------------------------------------------------------------------
73 
74 int movej(float fTargetPos[NUM_JOINT], float fTargetVel, float fTargetAcc, float fTargetTime = 0.f, float fBlendingRadius = 0.f,
75  int nMoveMode = MOVE_MODE_ABSOLUTE, int nBlendingType = BLENDING_SPEED_TYPE_DUPLICATE, int nSyncType = 0)
76 {
77  //ros::ServiceClient srvMoveJoint = nh.serviceClient<dsr_msgs::MoveJoint>("/dsr01m1013/motion/move_joint");
78  ros::NodeHandlePtr node = boost::make_shared<ros::NodeHandle>();
79  ros::ServiceClient srvMoveJoint = node->serviceClient<dsr_msgs::MoveJoint>("/"+ROBOT_ID + ROBOT_MODEL+"/motion/move_joint");
80 
81  dsr_msgs::MoveJoint srv;
82 
83  for(int i=0; i<NUM_JOINT; i++)
84  srv.request.pos[i] = fTargetPos[i];
85  srv.request.vel = fTargetVel;
86  srv.request.acc = fTargetAcc;
87  srv.request.time = fTargetTime;
88  srv.request.radius = fBlendingRadius;
89  srv.request.mode = nMoveMode;
90  srv.request.blendType = nBlendingType;
91  srv.request.syncType = nSyncType;
92 
93  if(srvMoveJoint.call(srv))
94  {
95  //ROS_INFO("receive srv, srv.response.success: %ld\n", (long int)srv.response.success);
96  return (srv.response.success);
97  }
98  else
99  {
100  ROS_ERROR("Failed to call service dr_control_service : move_joint\n");
101  ros::shutdown();
102  return -1;
103  }
104 }
105 int amovej(float fTargetPos[NUM_JOINT], float fTargetVel, float fTargetAcc, float fTargetTime = 0.f, float fBlendingRadius = 0.f,
106  int nMoveMode = MOVE_MODE_ABSOLUTE, int nBlendingType = BLENDING_SPEED_TYPE_DUPLICATE, int nSyncType = 1)
107 {
108  return movej(fTargetPos, fTargetVel, fTargetAcc, fTargetTime, fBlendingRadius, nMoveMode, nBlendingType, /*nSyncType=*/ 1);
109 }
110 
111 int movel(float fTargetPos[NUM_JOINT], float fTargetVel[2], float fTargetAcc[2], float fTargetTime = 0.f, float fBlendingRadius = 0.f,
112  int nMoveReference = MOVE_REFERENCE_BASE, int nMoveMode = MOVE_MODE_ABSOLUTE, int nBlendingType = BLENDING_SPEED_TYPE_DUPLICATE, int nSyncType = 0)
113 {
114  ros::NodeHandlePtr node = boost::make_shared<ros::NodeHandle>();
115  ros::ServiceClient srvMoveLine = node->serviceClient<dsr_msgs::MoveLine>("/"+ROBOT_ID +ROBOT_MODEL+"/motion/move_line");
116  dsr_msgs::MoveLine srv;
117 
118  for(int i=0; i<NUM_JOINT; i++)
119  srv.request.pos[i] = fTargetPos[i];
120  for(int i=0; i<2; i++){
121  srv.request.vel[i] = fTargetVel[i];
122  srv.request.acc[i] = fTargetAcc[i];
123  }
124  srv.request.time = fTargetTime;
125  srv.request.radius = fBlendingRadius;
126  srv.request.ref = nMoveReference;
127  srv.request.mode = nMoveMode;
128  srv.request.blendType = nBlendingType;
129  srv.request.syncType = nSyncType;
130 
131  if(srvMoveLine.call(srv))
132  {
133  //ROS_INFO("receive srv, srv.response.success: %ld\n", (long int)srv.response.success);
134  return (srv.response.success);
135  }
136  else
137  {
138  ROS_ERROR("Failed to call service dr_control_service : move_line\n");
139  ros::shutdown();
140  return -1;
141  }
142 }
143 int amovel(float fTargetPos[NUM_JOINT], float fTargetVel[2], float fTargetAcc[2], float fTargetTime = 0.f, float fBlendingRadius = 0.f,
144  int nMoveReference = MOVE_REFERENCE_BASE, int nMoveMode = MOVE_MODE_ABSOLUTE, int nBlendingType = BLENDING_SPEED_TYPE_DUPLICATE, int nSyncType = 1)
145 {
146  return movel(fTargetPos, fTargetVel, fTargetAcc, fTargetTime, fBlendingRadius, nMoveReference, nMoveMode, nBlendingType, /*nSyncType=*/ 1);
147 }
148 
149 
150 int movejx(float fTargetPos[NUM_TASK], float fTargetVel, float fTargetAcc, float fTargetTime=0.f, float fBlendingRadius = 0.f,
151  int nMoveReference = MOVE_REFERENCE_BASE, int nMoveMode = MOVE_MODE_ABSOLUTE, int nBlendingType = BLENDING_SPEED_TYPE_DUPLICATE, int nSolSpace = 0, int nSyncType = 0)
152 {
153  ros::NodeHandlePtr node = boost::make_shared<ros::NodeHandle>();
154  ros::ServiceClient srvMoveJointx = node->serviceClient<dsr_msgs::MoveJointx>("/"+ROBOT_ID +ROBOT_MODEL+"/motion/move_jointx");
155  dsr_msgs::MoveJointx srv;
156 
157  for(int i=0; i<NUM_TASK; i++)
158  srv.request.pos[i] = fTargetPos[i];
159  srv.request.vel = fTargetVel;
160  srv.request.acc = fTargetAcc;
161  srv.request.time = fTargetTime;
162  srv.request.radius = fBlendingRadius;
163  srv.request.ref = nMoveReference;
164  srv.request.mode = nMoveMode;
165  srv.request.blendType = nBlendingType;
166  srv.request.sol = nSolSpace;
167 
168  if(srvMoveJointx.call(srv))
169  {
170  //ROS_INFO("receive srv, srv.response.success: %ld\n", (long int)srv.response.success);
171  return (srv.response.success);
172  }
173  else
174  {
175  ROS_ERROR("Failed to call service dr_control_service : move_jointx\n");
176  ros::shutdown();
177  return -1;
178  }
179 }
180 int amovejx(float fTargetPos[NUM_TASK], float fTargetVel, float fTargetAcc, float fTargetTime=0.f, float fBlendingRadius = 0.f,
181  int nMoveReference = MOVE_REFERENCE_BASE, int nMoveMode = MOVE_MODE_ABSOLUTE, int nBlendingType = BLENDING_SPEED_TYPE_DUPLICATE, int nSolSpace = 0, int nSyncType = 1)
182 {
183  return movejx(fTargetPos, fTargetVel, fTargetAcc, fTargetTime, fBlendingRadius, nMoveReference, nMoveMode, nBlendingType, nSolSpace, /*nSyncType=*/ 1);
184 }
185 
186 
187 int movec(float fTargetPos[2][NUM_TASK], float fTargetVel[2], float fTargetAcc[2], float fTargetTime = 0.f, float fBlendingRadius = 0.f,
188  int nMoveReference = MOVE_REFERENCE_BASE, int nMoveMode = MOVE_MODE_ABSOLUTE, int nBlendingType = BLENDING_SPEED_TYPE_DUPLICATE, int nSyncType = 0)
189 {
190  ros::NodeHandlePtr node = boost::make_shared<ros::NodeHandle>();
191  ros::ServiceClient srvMoveCircle = node->serviceClient<dsr_msgs::MoveCircle>("/"+ROBOT_ID +ROBOT_MODEL+"/motion/move_circle");
192 
193  dsr_msgs::MoveCircle srv;
194  std::vector<std_msgs::Float64MultiArray> poses;
195  std_msgs::Float64MultiArray pos;
196  for(int i=0; i<2; i++){
197  pos.data.clear();
198  for(int j = 0; j < NUM_TASK; j++){
199  pos.data.push_back(fTargetPos[i][j]);
200  }
201  poses.push_back(pos);
202  }
203  srv.request.pos = poses;
204 
205  for(int i=0; i<2; i++){
206  srv.request.vel[i] = fTargetVel[i];
207  srv.request.acc[i] = fTargetAcc[i];
208  }
209  srv.request.time = fTargetTime;
210  srv.request.radius = fBlendingRadius;
211  srv.request.ref = nMoveReference;
212  srv.request.mode = nMoveMode;
213  srv.request.blendType = nBlendingType;
214  srv.request.syncType = nSyncType;
215 
216  if(srvMoveCircle.call(srv))
217  {
218  //ROS_INFO("receive srv, srv.response.success: %ld\n", (long int)srv.response.success);
219  return (srv.response.success);
220  }
221  else
222  {
223  ROS_ERROR("Failed to call service dr_control_service : move_circle\n");
224  ros::shutdown();
225  return -1;
226  }
227 }
228 int amovec(float fTargetPos[2][NUM_TASK], float fTargetVel[2], float fTargetAcc[2], float fTargetTime = 0.f, float fBlendingRadius = 0.f,
229  int nMoveReference = MOVE_REFERENCE_BASE, int nMoveMode = MOVE_MODE_ABSOLUTE, int nBlendingType = BLENDING_SPEED_TYPE_DUPLICATE, int nSyncType = 1)
230 {
231  return movec(fTargetPos, fTargetVel, fTargetAcc, fTargetTime, fBlendingRadius, nMoveReference, nMoveMode, nBlendingType, /*nSyncType=*/ 1);
232 }
233 
234 
235 int movesj(float fTargetPos[MAX_SPLINE_POINT][NUM_JOINT], int nPosCount, float fTargetVel, float fTargetAcc, float fTargetTime = 0.f,
236  int nMoveMode = MOVE_MODE_ABSOLUTE, int nSyncType = 0)
237 {
238  ros::NodeHandlePtr node = boost::make_shared<ros::NodeHandle>();
239  ros::ServiceClient srvMoveSplineJoint = node->serviceClient<dsr_msgs::MoveSplineJoint>("/"+ROBOT_ID +ROBOT_MODEL+"/motion/move_spline_joint");
240  dsr_msgs::MoveSplineJoint srv;
241  std::vector<std_msgs::Float64MultiArray> poses;
242  std_msgs::Float64MultiArray pos;
243 
244  for(int i = 0; i < MAX_SPLINE_POINT; i++){
245  pos.data.clear();
246  for(int j = 0; j < NUM_JOINT; j++){
247  pos.data.push_back(fTargetPos[i][j]);
248  }
249  poses.push_back(pos);
250  }
251  srv.request.pos = poses;
252  srv.request.posCnt = nPosCount;
253 
254  srv.request.vel = fTargetVel;
255  srv.request.acc = fTargetAcc;
256  srv.request.time = fTargetTime;
257  srv.request.mode = nMoveMode;
258  srv.request.syncType = nSyncType;
259 
260  if(srvMoveSplineJoint.call(srv))
261  {
262  //ROS_INFO("receive srv, srv.response.success: %ld\n", (long int)srv.response.success);
263  return (srv.response.success);
264  }
265  else
266  {
267  ROS_ERROR("Failed to call service dr_control_service : move_spline_joint\n");
268  ros::shutdown();
269  return -1;
270  }
271 }
272 int amovesj(float fTargetPos[MAX_SPLINE_POINT][NUM_JOINT], int nPosCount, float fTargetVel, float fTargetAcc, float fTargetTime = 0.f,
273  int nMoveMode = MOVE_MODE_ABSOLUTE, int nSyncType = 1)
274 {
275  return movesj(fTargetPos, nPosCount, fTargetVel, fTargetAcc, fTargetTime, nMoveMode, /*nSyncType=*/ 1);
276 }
277 
278 
279 int movesx(float fTargetPos[MAX_SPLINE_POINT][NUM_TASK], int nPosCount, float fTargetVel[2], float fTargetAcc[2], float fTargetTime = 0.f,
280  int nMoveReference = MOVE_REFERENCE_BASE, int nMoveMode = MOVE_MODE_ABSOLUTE, int nVelOpt = SPLINE_VELOCITY_OPTION_DEFAULT, int nSyncType = 0)
281 {
282  ros::NodeHandlePtr node = boost::make_shared<ros::NodeHandle>();
283  ros::ServiceClient srvMoveSplineTask = node->serviceClient<dsr_msgs::MoveSplineTask>("/"+ROBOT_ID +ROBOT_MODEL+"/motion/move_spline_task");
284  dsr_msgs::MoveSplineTask srv;
285  std::vector<std_msgs::Float64MultiArray> poses;
286  std_msgs::Float64MultiArray pos;
287 
288  for(int i = 0; i < MAX_SPLINE_POINT; i++){
289  pos.data.clear();
290  for(int j = 0; j < NUM_TASK; j++){
291  pos.data.push_back(fTargetPos[i][j]);
292  }
293  poses.push_back(pos);
294  }
295  srv.request.pos = poses;
296  srv.request.posCnt = nPosCount;
297 
298  for(int i=0; i<2; i++){
299  srv.request.vel[i] = fTargetVel[i];
300  srv.request.acc[i] = fTargetAcc[i];
301  }
302  srv.request.time = fTargetTime;
303  srv.request.ref = nMoveReference;
304  srv.request.mode = nMoveMode;
305  srv.request.opt = nVelOpt;
306  srv.request.syncType = nSyncType;
307 
308  if(srvMoveSplineTask.call(srv))
309  {
310  //ROS_INFO("receive srv, srv.response.success: %ld\n", (long int)srv.response.success);
311  return (srv.response.success);
312  }
313  else
314  {
315  ROS_ERROR("Failed to call service dr_control_service : move_spline_task\n");
316  ros::shutdown();
317  return -1;
318  }
319 }
320 int amovesx(float fTargetPos[MAX_SPLINE_POINT][NUM_TASK], int nPosCount, float fTargetVel[2], float fTargetAcc[2], float fTargetTime = 0.f,
321  int nMoveReference = MOVE_REFERENCE_BASE, int nMoveMode = MOVE_MODE_ABSOLUTE, int nVelOpt = SPLINE_VELOCITY_OPTION_DEFAULT, int nSyncType = 1)
322 {
323  return movesx(fTargetPos, nPosCount, fTargetVel, fTargetAcc, fTargetTime, nMoveReference, nMoveMode, nVelOpt, /*nSyncType=*/ 1);
324 }
325 
326 
327 int moveb(MOVE_POSB* fTargetPos, int nPosCount, float fTargetVel[2], float fTargetAcc[2], float fTargetTime = 0.f,
328  int nMoveReference = MOVE_REFERENCE_BASE, int nMoveMode = MOVE_MODE_ABSOLUTE, int nSyncType = 0)
329 {
330  ros::NodeHandlePtr node = boost::make_shared<ros::NodeHandle>();
331  ros::ServiceClient srvMoveBlending = node->serviceClient<dsr_msgs::MoveBlending>("/"+ROBOT_ID +ROBOT_MODEL+"/motion/move_blending");
332  dsr_msgs::MoveBlending srv;
333 
334  std::vector<std_msgs::Float64MultiArray> segments;
335  std_msgs::Float64MultiArray segment;
336 
337  for(int i=0; i<nPosCount; i++){
338  segment.data.clear();
339 
340  for(int j=0; j<NUM_TASK; j++)
341  segment.data.push_back( fTargetPos[i]._fTargetPos[0][j]);
342  for(int j=0; j<NUM_TASK; j++)
343  segment.data.push_back( fTargetPos[i]._fTargetPos[1][j]);
344 
345  segment.data.push_back( fTargetPos[i]._iBlendType );
346  segment.data.push_back( fTargetPos[i]._fBlendRad );
347 
348  segments.push_back(segment);
349  }
350  srv.request.segment = segments;
351  srv.request.posCnt = nPosCount;
352 
353  for(int i=0; i<2; i++){
354  srv.request.vel[i] = fTargetVel[i];
355  srv.request.acc[i] = fTargetAcc[i];
356  }
357  srv.request.time = fTargetTime;
358  srv.request.ref = nMoveReference;
359  srv.request.mode = nMoveMode;
360  srv.request.syncType = nSyncType;
361 
362  if(srvMoveBlending.call(srv))
363  {
364  //ROS_INFO("receive srv, srv.response.success: %ld\n", (long int)srv.response.success);
365  return (srv.response.success);
366  }
367  else
368  {
369  ROS_ERROR("Failed to call service dr_control_service : move_spline_blending\n");
370  ros::shutdown();
371  return -1;
372  }
373 
374  return 0;
375 }
376 int amoveb(MOVE_POSB* fTargetPos, int nPosCount, float fTargetVel[2], float fTargetAcc[2], float fTargetTime = 0.f,
377  int nMoveReference = MOVE_REFERENCE_BASE, int nMoveMode = MOVE_MODE_ABSOLUTE, int nSyncType = 1)
378 {
379  return moveb(fTargetPos, nPosCount, fTargetVel, fTargetAcc, fTargetTime, nMoveReference, nMoveMode, /*nSyncType=*/ 1);
380 }
381 
382 
383 int move_spiral(float fRevolution, float fMaxRadius, float fMaxLength, float fTargetVel[2], float fTargetAcc[2], float fTargetTime = 0.f,
384  int nTaskAxis = TASK_AXIS_Z, int nMoveReference = MOVE_REFERENCE_TOOL, int nSyncType = 0)
385 {
386  ros::NodeHandlePtr node = boost::make_shared<ros::NodeHandle>();
387  ros::ServiceClient srvMoveSpiral = node->serviceClient<dsr_msgs::MoveSpiral>("/"+ROBOT_ID +ROBOT_MODEL+"/motion/move_spiral");
388  dsr_msgs::MoveSpiral srv;
389 
390  srv.request.revolution = fRevolution;
391  srv.request.maxRadius = fMaxRadius;
392  srv.request.maxLength = fMaxLength;
393  for(int i=0; i<2; i++){
394  srv.request.vel[i] = fTargetVel[i];
395  srv.request.acc[i] = fTargetAcc[i];
396  }
397  srv.request.time = fTargetTime;
398  srv.request.taskAxis = nTaskAxis;
399  srv.request.ref = nMoveReference;
400  srv.request.syncType = nSyncType;
401 
402  if(srvMoveSpiral.call(srv)){
403  //ROS_INFO("receive srv, srv.response.success: %ld\n", (long int)srv.response.success);
404  return (srv.response.success);
405  }
406  else{
407  ROS_ERROR("Failed to call service dr_control_service : move_spiral\n");
408  ros::shutdown();
409  return -1;
410  }
411 }
412 int amove_spiral(float fRevolution, float fMaxRadius, float fMaxLength, float fTargetVel[2], float fTargetAcc[2], float fTargetTime = 0.f,
413  int nTaskAxis = TASK_AXIS_Z, int nMoveReference = MOVE_REFERENCE_TOOL, int nSyncType = 1)
414 {
415  return move_spiral(fRevolution, fMaxRadius, fMaxLength, fTargetVel, fTargetAcc, fTargetTime, nTaskAxis, nMoveReference, /*nSyncType=*/ 1);
416 }
417 
418 
419 int move_periodic(float fAmplitude[NUM_TASK], float fPeriodic[NUM_TASK], float fAccelTime = 0.f, int nRepeat = 1, int nMoveReference = MOVE_REFERENCE_TOOL, int nSyncType = 0)
420 {
421  ros::NodeHandlePtr node = boost::make_shared<ros::NodeHandle>();
422  ros::ServiceClient srvMovePeriodic = node->serviceClient<dsr_msgs::MovePeriodic>("/"+ROBOT_ID +ROBOT_MODEL+"/motion/move_periodic");
423  dsr_msgs::MovePeriodic srv;
424 
425  for(int i=0; i<NUM_TASK; i++){
426  srv.request.amp[i] = fAmplitude[i];
427  srv.request.periodic[i] = fPeriodic[i];
428  }
429  srv.request.acc = fAccelTime;
430  srv.request.repeat = nRepeat;
431  srv.request.ref = nMoveReference;
432  srv.request.syncType = nSyncType;
433 
434  if(srvMovePeriodic.call(srv))
435  {
436  //ROS_INFO("receive srv, srv.response.success: %ld\n", (long int)srv.response.success);
437  return (srv.response.success);
438  }
439  else
440  {
441  ROS_ERROR("Failed to call service dr_control_service : move_periodic\n");
442  ros::shutdown();
443  return -1;
444  }
445 
446  return 0;
447 }
448 int amove_periodic(float fAmplitude[NUM_TASK], float fPeriodic[NUM_TASK], float fAccelTime = 0.f, int nRepeat = 1, int nMoveReference = MOVE_REFERENCE_TOOL, int nSyncType = 1)
449 {
450  return move_periodic(fAmplitude, fPeriodic, fAccelTime, nRepeat, nMoveReference, /*nSyncType=*/ 1);
451 }
452 
453 
454 int move_wait()
455 {
456  ros::NodeHandlePtr node = boost::make_shared<ros::NodeHandle>();
457  ros::ServiceClient srvMoveWait = node->serviceClient<dsr_msgs::MoveWait>("/"+ROBOT_ID +ROBOT_MODEL+"/motion/move_wait");
458  dsr_msgs::MoveWait srv;
459 
460  if(srvMoveWait.call(srv))
461  {
462  //ROS_INFO("receive srv, srv.response.success: %ld\n", (long int)srv.response.success);
463  return (srv.response.success);
464  }
465  else
466  {
467  ROS_ERROR("Failed to call service dr_control_service : move_wait\n");
468  ros::shutdown();
469  return -1;
470  }
471 
472  return 0;
473 }
474 
475 
477 
478 void thread_subscriber()
479 {
480  ros::NodeHandlePtr node = boost::make_shared<ros::NodeHandle>();
481  ros::MultiThreadedSpinner spinner(2);
482  spinner.spin();
483 }
484 
485 void SigHandler(int sig)
486 {
487  // Do some custom action.
488  // For example, publish a stop message to some other nodes.
489 
490  // All the default sigint handler does is call shutdown()
491  ROS_INFO("shutdown time! sig=%d",sig);
492  ROS_INFO("shutdown time! sig=%d",sig);
493  ROS_INFO("shutdown time! sig=%d",sig);
494  //ros::ServiceClient srvMoveStop = nh.serviceClient<dsr_msgs::MoveStop>("/dsr01m1013/motion/move_stop");
495 
496  ros::NodeHandlePtr node = boost::make_shared<ros::NodeHandle>();
497  ros::Publisher pubRobotStop = node->advertise<dsr_msgs::RobotStop>("/"+ROBOT_ID +ROBOT_MODEL+"/stop",100);
498 
499  dsr_msgs::RobotStop msg;
500 
501  msg.stop_mode = STOP_TYPE_QUICK;
502  pubRobotStop.publish(msg);
503 
504  ros::shutdown();
505 }
506 
507 int main(int argc, char** argv)
508 {
509  //----- set target robot ---------------
510  string my_robot_id = "dsr01";
511  string my_robot_model = "m1013";
512  if(1 == argc){
513  ROS_INFO("default arguments: dsr01 m1013");
514  }
515  else{
516  if(3 != argc){
517  ROS_ERROR("invalid arguments: <ns> <model> (ex) dsr01 m1013");
518  exit(1);
519  }
520  for (int i = 1; i < argc; i++){
521  printf("argv[%d] = %s\n", i, argv[i]);
522  }
523  my_robot_id = argv[1];
524  my_robot_model = argv[2];
525  }
526  //std::cout << "my_robot_id= " << my_robot_id << ", my_robot_model= " << my_robot_model << endl;
527  SET_ROBOT(my_robot_id, my_robot_model);
528 
529  //----- init ROS ----------------------
530  int rate_sub = 1; // 1 Hz = 1 sec
531  int nPubRate = 100; // 100 Hz (10ms)
532  int i=0, nRes=0;
533  ros::init(argc, argv, "dsr_service_motion_basic_cpp", ros::init_options::NoSigintHandler);
534  ros::NodeHandle nh("~");
535  // Override the default ros sigint handler.
536  // This must be set after the first NodeHandle is created.
537  signal(SIGINT, SigHandler);
538  ros::Publisher pubRobotStop = nh.advertise<dsr_msgs::RobotStop>("/"+ROBOT_ID +ROBOT_MODEL+"/stop",10);
539 
540  // spawn another thread
541  boost::thread thread_sub(thread_subscriber);
542 
543  /*
544  if(argc !=3)
545  {
546  ROS_INFO("cmd : rosrun dsr_control dsr_control_service arg0 arg1");
547  ROS_INFO("arg0: double number, arg1: double number");
548  return 1;
549  }
550  */
551 
553  float velx[2]={250.0, 80.625}; // 태스크 속도를 250(mm/sec), 80.625(deg/sec)로 설정
554  float accx[2]={1000.0, 322.5}; // 태스크 가속도를 1000(mm/sec2), 322.5(deg/sec2)로 설정
555 
556  float j1[6]={0.0, 0.0, 90.0, 0.0, 90.0, 0.0}; //joint
557  float sj1[2][6]={{10.00, 0.00, 0.00, 0.00, 10.00, 20.00},{15.00, 0.00, -10.00, 0.00, 10.00, 20.00}};
558  float x1[6]={0.0, 0.0, -100.0, 0.0, 0.0, 0.0}; //task
559  float x2[6]={545,100,514,0,-180,0}; //jx task
560  float cx1[2][6]={{544.00, 100.00, 500.00, 0.00, -180.00, 0.00},{543.00, 106.00, 479.00, 7.00, -180.00, 7.00}};
561  float sx1[2][6]={{10.00, -10.00, 20.00, 0.00, 10.00, 0.00},{15.00, 10.00, -10.00, 0.00, 10.00, 0.00}};
562  float bx1[2][6]={{564.00, 200.00, 690.00, 0.00, 180.00, 0.00},{0, 0, 0, 0, 0, 0}};
563  float bx2[2][6]={{564.00, 100.00, 590.00, 0.00, 180.00, 0.00},{564.00, 150.00, 590.00, 0.00, 180.00, 0.00}};
564 
565  float amp[6]={10.00, 0.00, 20.00, 0.00, 0.50, 0.00};
566  float period[6]={1.00, 0.00, 1.50, 0.00, 0.00, 0.00};
567  MOVE_POSB posb[2];
568 
569  while(ros::ok())
570  {
571  /*
572  set_velj(60.0)
573  set_accj(100.0)
574  set_velx(250.0, 80.625)
575  set_accx(1000.0, 322.5)
576  gLoopCountRev = 0 */
577  //movej(posj(0.00, 0.00, 90.00, 0.00, 90.00, 0.00), radius=0.00, ra=DR_MV_RA_DUPLICATE)
578  movej(j1, 60, 30);
579  //movel(posx(0.00, 0.00, -100.00, 0.00, 0.00, 0.00), radius=0.00, ref=DR_BASE, mod=DR_MV_MOD_REL,ra=DR_MV_RA_DUPLICATE)
580  movel(x1, velx, accx, 0, 0, MOVE_REFERENCE_BASE, MOVE_MODE_RELATIVE);
581  //movejx
582  movejx(x2, 60, 30, 2, 0, MOVE_REFERENCE_BASE, MOVE_MODE_ABSOLUTE, BLENDING_SPEED_TYPE_DUPLICATE, 2);
583  //movec(posx(544.00, 100.00, 500.00, 0.00, -180.00, 0.00), posx(543.00, 106.00, 479.00, 7.00, -180.00, 7.00), radius=0.00, angle=[0.00,0.00],ra=DR_MV_RA_DUPLICATE)
584  movec(cx1, velx, accx);
585  //movesj([posj(10.00, 0.00, 0.00, 0.00, 10.00, 20.00), posj(15.00, 0.00, -10.00, 0.00, 10.00, 20.00)], mod=DR_MV_MOD_REL)
586  movesj(sj1, 2, 60, 30, 0, MOVE_MODE_RELATIVE);
587  //movesx([posx(10.00, -10.00, 20.00, 0.00, 10.00, 0.00), posx(15.00, 10.00, -10.00, 0.00, 10.00, 0.00)], mod=DR_MV_MOD_REL)
588  movesx(sx1, 2, velx, accx, 0, MOVE_MODE_RELATIVE);
589  //moveb([posb(DR_LINE, posx(564.00, 200.00, 690.00, 0.00, 180.00, 0.00), radius=40.0), posb(DR_CIRCLE, posx(564.00, 100.00, 590.00, 0.00, 180.00, 0.00), posx(564.00, 150.00, 590.00, 0.00, 180.00, 0.00), radius=20.0)], ref=DR_BASE, mod=DR_MV_MOD_ABS)
590  for(int i=0; i<2; i++){
591  for(int j=0; j<6; j++){
592  posb[0]._fTargetPos[i][j] = bx1[i][j];
593  posb[1]._fTargetPos[i][j] = bx2[i][j];
594  }
595  }
596 
597  posb[0]._iBlendType = 0; // LINE
598  posb[1]._iBlendType = 1; // CIRCLE
599 
600  posb[0]._fBlendRad = 40.0;
601  posb[1]._fBlendRad = 20.0;
602 
603  moveb(posb, 2, velx, accx);
604  //move_spiral(rev=1.00, rmax=20.00, lmax=20.00, time=5.00, axis=DR_AXIS_Z, ref=DR_TOOL)
605  move_spiral(1.00, 20.00, 20.00, velx, accx, 5, TASK_AXIS_Z, MOVE_REFERENCE_TOOL);
606  //move_periodic(amp=[10.00, 0.00, 20.00, 0.00, 0.50, 0.00], period=[1.00, 0.00, 1.50, 0.00, 0.00, 0.00], atime=0.50, repeat=3, ref=DR_BASE)
607  move_periodic(amp, period, 0.5, 3, MOVE_REFERENCE_BASE);
608 
609  //---- async motions ---------------------------------------------------------------------------------
610  amovej(j1, 60, 30, 0, 0, MOVE_MODE_ABSOLUTE, BLENDING_SPEED_TYPE_DUPLICATE);
611  move_wait();
612 
613  amovel(x1, velx, accx, 0, 0, MOVE_REFERENCE_BASE, MOVE_MODE_RELATIVE, BLENDING_SPEED_TYPE_DUPLICATE);
614  move_wait();
615 
616  amovejx(x2, 60, 30, 2, MOVE_MODE_ABSOLUTE,MOVE_REFERENCE_BASE, 0, BLENDING_SPEED_TYPE_DUPLICATE, 2);
617  move_wait();
618 
619  amovec(cx1, velx, accx, 0, 0, MOVE_REFERENCE_BASE, MOVE_MODE_ABSOLUTE, BLENDING_SPEED_TYPE_DUPLICATE);
620  move_wait();
621 
622  amovesj(sj1, 2, 60, 30, 0, MOVE_MODE_RELATIVE);
623  move_wait();
624 
625  amovesx(sx1, 2, velx, accx, 0, MOVE_MODE_RELATIVE, MOVE_REFERENCE_BASE, SPLINE_VELOCITY_OPTION_DEFAULT);
626  move_wait();
627 
628  amoveb(posb, 2, velx, accx, 0, MOVE_MODE_ABSOLUTE, MOVE_REFERENCE_BASE);
629  move_wait();
630 
631  amove_spiral(1.00, 20.00, 20.00, velx, accx, 5, TASK_AXIS_Z, MOVE_REFERENCE_TOOL);
632  move_wait();
633 
634  amove_periodic(amp, period, 0.5, 3, MOVE_REFERENCE_BASE);
635  move_wait();
636  }
637 
638  ros::shutdown();
639  // wait the second thread to finish
641  ROS_INFO("dsr_service_motion_basic_cpp finished !!!!!!!!!!!!!!!!!!!!!");
642  return 0;
643 }
movejx
int movejx(float fTargetPos[NUM_TASK], float fTargetVel, float fTargetAcc, float fTargetTime=0.f, float fBlendingRadius=0.f, int nMoveReference=MOVE_REFERENCE_BASE, int nMoveMode=MOVE_MODE_ABSOLUTE, int nBlendingType=BLENDING_SPEED_TYPE_DUPLICATE, int nSolSpace=0, int nSyncType=0)
Definition: dsr_service_motion_basic.cpp:150
movesx
int movesx(float fTargetPos[MAX_SPLINE_POINT][NUM_TASK], int nPosCount, float fTargetVel[2], float fTargetAcc[2], float fTargetTime=0.f, int nMoveReference=MOVE_REFERENCE_BASE, int nMoveMode=MOVE_MODE_ABSOLUTE, int nVelOpt=SPLINE_VELOCITY_OPTION_DEFAULT, int nSyncType=0)
Definition: dsr_service_motion_basic.cpp:279
ros::NodeHandle
amp
float amp[6]
Definition: m1013x2_sync2.cpp:61
ros::init_options::NoSigintHandler
NoSigintHandler
move_wait
int move_wait()
Definition: dsr_service_motion_basic.cpp:454
amovel
int amovel(float fTargetPos[NUM_JOINT], float fTargetVel[2], float fTargetAcc[2], float fTargetTime=0.f, float fBlendingRadius=0.f, int nMoveReference=MOVE_REFERENCE_BASE, int nMoveMode=MOVE_MODE_ABSOLUTE, int nBlendingType=BLENDING_SPEED_TYPE_DUPLICATE, int nSyncType=1)
Definition: dsr_service_motion_basic.cpp:143
velx
float velx[2]
Definition: m1013x2_sync2.cpp:52
SET_ROBOT
void SET_ROBOT(string id, string model)
Definition: dsr_service_motion_basic.cpp:71
amovesj
int amovesj(float fTargetPos[MAX_SPLINE_POINT][NUM_JOINT], int nPosCount, float fTargetVel, float fTargetAcc, float fTargetTime=0.f, int nMoveMode=MOVE_MODE_ABSOLUTE, int nSyncType=1)
Definition: dsr_service_motion_basic.cpp:272
movesj
int movesj(float fTargetPos[MAX_SPLINE_POINT][NUM_JOINT], int nPosCount, float fTargetVel, float fTargetAcc, float fTargetTime=0.f, int nMoveMode=MOVE_MODE_ABSOLUTE, int nSyncType=0)
Definition: dsr_service_motion_basic.cpp:235
ros::init
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
amovesx
int amovesx(float fTargetPos[MAX_SPLINE_POINT][NUM_TASK], int nPosCount, float fTargetVel[2], float fTargetAcc[2], float fTargetTime=0.f, int nMoveReference=MOVE_REFERENCE_BASE, int nMoveMode=MOVE_MODE_ABSOLUTE, int nVelOpt=SPLINE_VELOCITY_OPTION_DEFAULT, int nSyncType=1)
Definition: dsr_service_motion_basic.cpp:320
movec
int movec(float fTargetPos[2][NUM_TASK], float fTargetVel[2], float fTargetAcc[2], float fTargetTime=0.f, float fBlendingRadius=0.f, int nMoveReference=MOVE_REFERENCE_BASE, int nMoveMode=MOVE_MODE_ABSOLUTE, int nBlendingType=BLENDING_SPEED_TYPE_DUPLICATE, int nSyncType=0)
Definition: dsr_service_motion_basic.cpp:187
period
float period[6]
Definition: m1013x2_sync2.cpp:62
moveb
int moveb(MOVE_POSB *fTargetPos, int nPosCount, float fTargetVel[2], float fTargetAcc[2], float fTargetTime=0.f, int nMoveReference=MOVE_REFERENCE_BASE, int nMoveMode=MOVE_MODE_ABSOLUTE, int nSyncType=0)
Definition: dsr_service_motion_basic.cpp:327
movel
int movel(float fTargetPos[NUM_JOINT], float fTargetVel[2], float fTargetAcc[2], float fTargetTime=0.f, float fBlendingRadius=0.f, int nMoveReference=MOVE_REFERENCE_BASE, int nMoveMode=MOVE_MODE_ABSOLUTE, int nBlendingType=BLENDING_SPEED_TYPE_DUPLICATE, int nSyncType=0)
Definition: dsr_service_motion_basic.cpp:111
movej
int movej(float fTargetPos[NUM_JOINT], float fTargetVel, float fTargetAcc, float fTargetTime=0.f, float fBlendingRadius=0.f, int nMoveMode=MOVE_MODE_ABSOLUTE, int nBlendingType=BLENDING_SPEED_TYPE_DUPLICATE, int nSyncType=0)
Definition: dsr_service_motion_basic.cpp:74
boost::shared_ptr< NodeHandle >
accx
float accx[2]
Definition: m1013x2_sync2.cpp:53
main
int main(int argc, char **argv)
Definition: dsr_service_motion_basic.cpp:507
amovejx
int amovejx(float fTargetPos[NUM_TASK], float fTargetVel, float fTargetAcc, float fTargetTime=0.f, float fBlendingRadius=0.f, int nMoveReference=MOVE_REFERENCE_BASE, int nMoveMode=MOVE_MODE_ABSOLUTE, int nBlendingType=BLENDING_SPEED_TYPE_DUPLICATE, int nSolSpace=0, int nSyncType=1)
Definition: dsr_service_motion_basic.cpp:180
amoveb
int amoveb(MOVE_POSB *fTargetPos, int nPosCount, float fTargetVel[2], float fTargetAcc[2], float fTargetTime=0.f, int nMoveReference=MOVE_REFERENCE_BASE, int nMoveMode=MOVE_MODE_ABSOLUTE, int nSyncType=1)
Definition: dsr_service_motion_basic.cpp:376
ROS_INFO
#define ROS_INFO(...)
move_spiral
int move_spiral(float fRevolution, float fMaxRadius, float fMaxLength, float fTargetVel[2], float fTargetAcc[2], float fTargetTime=0.f, int nTaskAxis=TASK_AXIS_Z, int nMoveReference=MOVE_REFERENCE_TOOL, int nSyncType=0)
Definition: dsr_service_motion_basic.cpp:383
ros::ok
ROSCPP_DECL bool ok()
ros::NodeHandle::advertise
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
move_periodic
int move_periodic(float fAmplitude[NUM_TASK], float fPeriodic[NUM_TASK], float fAccelTime=0.f, int nRepeat=1, int nMoveReference=MOVE_REFERENCE_TOOL, int nSyncType=0)
Definition: dsr_service_motion_basic.cpp:419
thread_subscriber
void thread_subscriber()
Definition: dsr_service_motion_basic.cpp:478
amove_periodic
int amove_periodic(float fAmplitude[NUM_TASK], float fPeriodic[NUM_TASK], float fAccelTime=0.f, int nRepeat=1, int nMoveReference=MOVE_REFERENCE_TOOL, int nSyncType=1)
Definition: dsr_service_motion_basic.cpp:448
amove_spiral
int amove_spiral(float fRevolution, float fMaxRadius, float fMaxLength, float fTargetVel[2], float fTargetAcc[2], float fTargetTime=0.f, int nTaskAxis=TASK_AXIS_Z, int nMoveReference=MOVE_REFERENCE_TOOL, int nSyncType=1)
Definition: dsr_service_motion_basic.cpp:412
amovec
int amovec(float fTargetPos[2][NUM_TASK], float fTargetVel[2], float fTargetAcc[2], float fTargetTime=0.f, float fBlendingRadius=0.f, int nMoveReference=MOVE_REFERENCE_BASE, int nMoveMode=MOVE_MODE_ABSOLUTE, int nBlendingType=BLENDING_SPEED_TYPE_DUPLICATE, int nSyncType=1)
Definition: dsr_service_motion_basic.cpp:228
ros::shutdown
ROSCPP_DECL void shutdown()
ROBOT_ID
string ROBOT_ID
Definition: dsr_service_motion_basic.cpp:69
amovej
int amovej(float fTargetPos[NUM_JOINT], float fTargetVel, float fTargetAcc, float fTargetTime=0.f, float fBlendingRadius=0.f, int nMoveMode=MOVE_MODE_ABSOLUTE, int nBlendingType=BLENDING_SPEED_TYPE_DUPLICATE, int nSyncType=1)
Definition: dsr_service_motion_basic.cpp:105
ROS_ERROR
#define ROS_ERROR(...)
ROBOT_MODEL
string ROBOT_MODEL
Definition: dsr_service_motion_basic.cpp:70
SigHandler
void SigHandler(int sig)
Definition: dsr_service_motion_basic.cpp:485

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