dsr_service_motion_simple.cpp
Go to the documentation of this file.
1 /*
2  * [c++ example simple] 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 
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 void SET_ROBOT(string id, string model) {ROBOT_ID = id; ROBOT_MODEL= model;}
22 //---------------------------------------------------------------------------
23 
25 void msgRobotState_cb(const dsr_msgs::RobotState::ConstPtr& msg)
26 {
27  static int sn_cnt =0;
28 
29  sn_cnt++;
30  if(0==(sn_cnt % 100))
31  {
32  ROS_INFO("________ ROBOT STATUS ________");
33  ROS_INFO(" robot_state : %d", msg->robot_state);
34  ROS_INFO(" robot_state_str : %s", msg->robot_state_str.c_str());
35  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]
36  ,msg->current_posj[3] ,msg->current_posj[4] ,msg->current_posj[5] );
37  //ROS_INFO(" io_control_box : %d", msg->io_control_box);
38  //ROS_INFO(" io_modbus : %d", msg->io_modbus);
39  //ROS_INFO(" error : %d", msg->error);
40  ROS_INFO(" access_control : %d", msg->access_control);
41  ROS_INFO(" homming_completed : %d", msg->homming_completed);
42  ROS_INFO(" tp_initialized : %d", msg->tp_initialized);
43  //ROS_INFO(" speed : %d", msg->speed);
44  ROS_INFO(" mastering_need : %d", msg->mastering_need);
45  ROS_INFO(" drl_stopped : %d", msg->drl_stopped);
46  ROS_INFO(" disconnected : %d", msg->disconnected);
47  }
48 }
49 
50 static void thread_subscriber()
51 {
52  ros::NodeHandlePtr node = boost::make_shared<ros::NodeHandle>();
55  ros::MultiThreadedSpinner spinner(2);
56  spinner.spin();
57 }
58 
59 void SigHandler(int sig)
60 {
61  // Do some custom action.
62  // For example, publish a stop message to some other nodes.
63 
64  // All the default sigint handler does is call shutdown()
65  ROS_INFO("shutdown time!");
66  ROS_INFO("shutdown time!");
67  ROS_INFO("shutdown time!");
68  //ros::ServiceClient srvMoveStop = nh.serviceClient<dsr_msgs::MoveStop>("/dsr01m1013/motion/move_stop");
69 
70  ros::NodeHandlePtr node = boost::make_shared<ros::NodeHandle>();
71  ros::Publisher pubRobotStop = node->advertise<dsr_msgs::RobotStop>("/"+ROBOT_ID +ROBOT_MODEL+"/stop",100);
72 
73  dsr_msgs::RobotStop msg;
74 
75  msg.stop_mode = STOP_TYPE_QUICK;
76  pubRobotStop.publish(msg);
77 
78  ros::shutdown();
79 }
80 
81 int main(int argc, char** argv)
82 {
83  //----- set target robot ---------------
84  string my_robot_id = "dsr01";
85  string my_robot_model = "m1013";
86  if(1 == argc){
87  ROS_INFO("default arguments: dsr01 m1013");
88  }
89  else{
90  if(3 != argc){
91  ROS_ERROR("invalid arguments: <ns> <model> (ex) dsr01 m1013");
92  exit(1);
93  }
94  for (int i = 1; i < argc; i++){
95  printf("argv[%d] = %s\n", i, argv[i]);
96  }
97  my_robot_id = argv[1];
98  my_robot_model = argv[2];
99  }
100  //std::cout << "my_robot_id= " << my_robot_id << ", my_robot_model= " << my_robot_model << endl;
101  SET_ROBOT(my_robot_id, my_robot_model);
102 
103  //----- init ROS ----------------------
104  ros::init(argc, argv, "dsr_service_test_cpp", ros::init_options::NoSigintHandler);
105  ros::NodeHandle nh("~");
106  // Override the default ros sigint handler.
107  // This must be set after the first NodeHandle is created.
108  signal(SIGINT, SigHandler);
109  ros::Publisher pubRobotStop = nh.advertise<dsr_msgs::RobotStop>("/"+ROBOT_ID +ROBOT_MODEL+"/stop",10);
111 
112  //----- create DsrRobot ---------------
113  CDsrRobot robot(nh, my_robot_id, my_robot_model);
114 
115  // run subscriber thread (for monitoring)
116  boost::thread thread_sub(thread_subscriber);
117 
119  float velx[2]={250.0, 80.625}; // 태스크 속도를 250(mm/sec), 80.625(deg/sec)로 설정
120  float accx[2]={1000.0, 322.5}; // 태스크 가속도를 1000(mm/sec2), 322.5(deg/sec2)로 설정
121 
122  float j1[6]={0.0, 0.0, 90.0, 0.0, 90.0, 0.0}; //joint
123  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}};
124  float x1[6]={0.0, 0.0, -100.0, 0.0, 0.0, 0.0}; //task
125  float x2[6]={545,100,514,0,-180,0}; //jx task
126  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}};
127  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}};
128  float bx1[2][6]={{564.00, 200.00, 690.00, 0.00, 180.00, 0.00},{0, 0, 0, 0, 0, 0}};
129  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}};
130 
131  float amp[6]={10.00, 0.00, 20.00, 0.00, 0.50, 0.00};
132  float period[6]={1.00, 0.00, 1.50, 0.00, 0.00, 0.00};
133  MOVE_POSB posb[2];
134 
135  while(ros::ok())
136  {
137  /*
138  set_velj(60.0)
139  set_accj(100.0)
140  set_velx(250.0, 80.625)
141  set_accx(1000.0, 322.5)
142  gLoopCountRev = 0 */
143  //movej(posj(0.00, 0.00, 90.00, 0.00, 90.00, 0.00), radius=0.00, ra=DR_MV_RA_DUPLICATE)
144  robot.movej(j1, 60, 30);
145  //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)
146  robot.movel(x1, velx, accx, 0, MOVE_MODE_RELATIVE);
147  //movejx
148  robot.movejx(x2, 60, 30, 2, MOVE_MODE_ABSOLUTE,MOVE_REFERENCE_BASE, 0, BLENDING_SPEED_TYPE_DUPLICATE, 2);
149  //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)
150  robot.movec(cx1, velx, accx);
151  //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)
152  robot.movesj(sj1, 2, 60, 30, 0, MOVE_MODE_RELATIVE);
153  //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)
154  robot.movesx(sx1, 2, velx, accx, 0, MOVE_MODE_RELATIVE);
155  //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)
156  for(int i=0; i<2; i++){
157  for(int j=0; j<6; j++){
158  posb[0]._fTargetPos[i][j] = bx1[i][j];
159  posb[1]._fTargetPos[i][j] = bx2[i][j];
160  }
161  }
162 
163  posb[0]._iBlendType = 0; // LINE
164  posb[1]._iBlendType = 1; // CIRCLE
165 
166  posb[0]._fBlendRad = 40.0;
167  posb[1]._fBlendRad = 20.0;
168 
169  robot.moveb(posb, 2, velx, accx);
170  //move_spiral(rev=1.00, rmax=20.00, lmax=20.00, time=5.00, axis=DR_AXIS_Z, ref=DR_TOOL)
171  robot.move_spiral(1.00, 20.00, 20.00, velx, accx, 5, TASK_AXIS_Z, MOVE_REFERENCE_TOOL);
172  //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)
173  robot.move_periodic(amp, period, 0.5, 3, MOVE_REFERENCE_BASE);
174  //amovej
175  robot.amovej(j1, 60, 30, 0, MOVE_MODE_ABSOLUTE, 0, BLENDING_SPEED_TYPE_DUPLICATE);
176  //wait
177  robot.move_wait();
178  //amovel
179  robot.amovel(x1, velx, accx, 0, MOVE_MODE_RELATIVE, MOVE_REFERENCE_BASE, 0, BLENDING_SPEED_TYPE_DUPLICATE);
180  //amovejx
181  robot.amovejx(x2, 60, 30, 2,MOVE_MODE_ABSOLUTE,MOVE_REFERENCE_BASE, 0, BLENDING_SPEED_TYPE_DUPLICATE, 2);
182  //wait
183  robot.move_wait();
184  //amovec
185  robot.amovec(cx1, velx, accx, 0, MOVE_MODE_ABSOLUTE, MOVE_REFERENCE_BASE, 0, BLENDING_SPEED_TYPE_DUPLICATE);
186  //wait
187  robot.move_wait();
188  //amovesj
189  robot.amovesj(sj1, 2, 60, 30, 0, MOVE_MODE_RELATIVE);
190  //wait
191  robot.move_wait();
192  //amovesx
193  robot.amovesx(sx1, 2, velx, accx, 0, MOVE_MODE_RELATIVE, MOVE_REFERENCE_BASE, SPLINE_VELOCITY_OPTION_DEFAULT);
194  //wait
195  robot.move_wait();
196  //amoveb
197  robot.amoveb(posb, 2, velx, accx, 0, MOVE_MODE_ABSOLUTE, MOVE_REFERENCE_BASE);
198  //wait
199  robot.move_wait();
200  //amovespiral
201  robot.amove_spiral(1.00, 20.00, 20.00, velx, accx, 5, TASK_AXIS_Z, MOVE_REFERENCE_TOOL);
202  //wait
203  robot.move_wait();
204  //amoveperiodic
205  robot.amove_periodic(amp, period, 0.5, 3, MOVE_REFERENCE_BASE);
206  //wait
207  robot.move_wait();
208  }
209  thread_sub.join();
210  // wait the second thread to finish
211  ROS_INFO("dsr_service_test finished !!!!!!!!!!!!!!!!!!!!!");
212  return 0;
213 }
ROBOT_MODEL
string ROBOT_MODEL
Definition: dsr_service_motion_simple.cpp:20
main
int main(int argc, char **argv)
Definition: dsr_service_motion_simple.cpp:81
ros::NodeHandle
amp
float amp[6]
Definition: m1013x2_sync2.cpp:61
ros::init_options::NoSigintHandler
NoSigintHandler
SET_ROBOT
void SET_ROBOT(string id, string model)
Definition: dsr_service_motion_simple.cpp:21
velx
float velx[2]
Definition: m1013x2_sync2.cpp:52
ros::init
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
period
float period[6]
Definition: m1013x2_sync2.cpp:62
DSR_Robot
boost::shared_ptr< NodeHandle >
accx
float accx[2]
Definition: m1013x2_sync2.cpp:53
SigHandler
void SigHandler(int sig)
Definition: dsr_service_motion_simple.cpp:59
ROS_INFO
#define ROS_INFO(...)
thread_subscriber
static void thread_subscriber()
Definition: dsr_service_motion_simple.cpp:50
ros::ok
ROSCPP_DECL bool ok()
ros::NodeHandle::advertise
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
msgRobotState_cb
void msgRobotState_cb(const dsr_msgs::RobotState::ConstPtr &msg)
Definition: dsr_service_motion_simple.cpp:25
ros::shutdown
ROSCPP_DECL void shutdown()
ROS_ERROR
#define ROS_ERROR(...)
ROBOT_ID
string ROBOT_ID
Definition: dsr_service_motion_simple.cpp:19

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