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

cpp
Author(s): Kab Kyoum Kim , Jin Hyuk Gong , Jeongwoo Lee
autogenerated on Thu Jun 20 2019 19:43:26