dsr_service_modbus_basic.cpp
Go to the documentation of this file.
1 /*
2  * [c++ example basic] modbus 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],
75  float fTargetVel,
76  float fTargetAcc,
77  float fTargetTime = 0.f,
78  int nMoveMode = MOVE_MODE_ABSOLUTE,
79  float fBlendingRadius = 0.f,
80  int nBlendingType = BLENDING_SPEED_TYPE_DUPLICATE,
81  int nSyncType = 0)
82 {
83 
84  //ros::ServiceClient srvMoveJoint = nh.serviceClient<dsr_msgs::MoveJoint>("/dsr01m1013/motion/move_joint");
85  ros::NodeHandlePtr node = boost::make_shared<ros::NodeHandle>();
86  ros::ServiceClient srvMoveJoint = node->serviceClient<dsr_msgs::MoveJoint>("/"+ROBOT_ID +ROBOT_MODEL+"/motion/move_joint");
87 
88  dsr_msgs::MoveJoint srv;
89 
90  for(int i=0; i<NUM_JOINT; i++)
91  srv.request.pos[i] = fTargetPos[i];
92  srv.request.vel = fTargetVel;
93  srv.request.acc = fTargetAcc;
94  srv.request.time = fTargetTime;
95  srv.request.mode = nMoveMode;
96  srv.request.radius = fBlendingRadius;
97  srv.request.blendType = nBlendingType;
98  srv.request.syncType = nSyncType;
99  if(srvMoveJoint.call(srv))
100  {
101  //ROS_INFO("receive srv, srv.response.success: %ld\n", (long int)srv.response.success);
102  return (srv.response.success);
103  }
104  else
105  {
106  ROS_ERROR("Failed to call service dr_control_service : move_joint\n");
107  ros::shutdown();
108  return -1;
109  }
110 
111  return 0;
112 }
113 
114 int set_modbus_output(string strName,
115  int nValue)
116 {
117  ros::NodeHandlePtr node = boost::make_shared<ros::NodeHandle>();
118  ros::ServiceClient srvSetModbusOutput = node->serviceClient<dsr_msgs::SetModbusOutput>("/"+ROBOT_ID +ROBOT_MODEL+"/modbus/set_modbus_output");
119  dsr_msgs::SetModbusOutput srv;
120 
121  srv.request.name = strName;
122  srv.request.value = nValue;
123 
124  if(srvSetModbusOutput.call(srv))
125  {
126  //ROS_INFO("receive srv, srv.response.success: %ld\n", (long int)srv.response.success);
127  return (srv.response.success);
128  }
129  else
130  {
131  ROS_ERROR("Failed to call service dr_control_service : set_modbus_output\n");
132  ros::shutdown();
133  return -1;
134  }
135  return 0;
136 }
137 
138 int get_modbus_input(string strName)
139 {
140  ros::NodeHandlePtr node = boost::make_shared<ros::NodeHandle>();
141  ros::ServiceClient srvGetModbusInput = node->serviceClient<dsr_msgs::GetModbusInput>("/"+ROBOT_ID +ROBOT_MODEL+"/modbus/get_modbus_input");
142  dsr_msgs::GetModbusInput srv;
143 
144  srv.request.name = strName;
145  if(srvGetModbusInput.call(srv))
146  {
147  //ROS_INFO("receive srv, srv.response.value: %ld\n", (long int)srv.response.value);
148  return (srv.response.value);
149  }
150  else
151  {
152  ROS_ERROR("Failed to call service dr_control_service : get_modbus_input\n");
153  ros::shutdown();
154  return -1;
155  }
156 }
157 
158 int config_create_modbus(string strName,
159  string strIP,
160  int nPort,
161  int nRegType,
162  int nRegIndex,
163  int nRegValue = 0)
164 {
165  ros::NodeHandlePtr node = boost::make_shared<ros::NodeHandle>();
166  ros::ServiceClient srvConfigCreateModbus = node->serviceClient<dsr_msgs::ConfigCreateModbus>("/"+ROBOT_ID +ROBOT_MODEL+"/modbus/config_create_modbus");
167  dsr_msgs::ConfigCreateModbus srv;
168 
169  srv.request.name = strName;
170  srv.request.ip = strIP;
171  srv.request.port = nPort;
172  srv.request.reg_type = nRegType;
173  srv.request.index = nRegIndex;
174  srv.request.value = nRegValue;
175 
176  if(srvConfigCreateModbus.call(srv))
177  {
178  //ROS_INFO("receive srv, srv.response.success: %ld\n", (long int)srv.response.success);
179  return (srv.response.success);
180  }
181  else
182  {
183  ROS_ERROR("Failed to call service dr_control_service : config_create_modbus\n");
184  ros::shutdown();
185  return -1;
186  }
187  return 0;
188 }
189 
190 int config_delete_modbus(string strName)
191 {
192  ros::NodeHandlePtr node = boost::make_shared<ros::NodeHandle>();
193  ros::ServiceClient srvConfigDeleteModbus = node->serviceClient<dsr_msgs::ConfigDeleteModbus>("/"+ROBOT_ID +ROBOT_MODEL+"/modbus/config_delete_modbus");
194  dsr_msgs::ConfigDeleteModbus srv;
195 
196  srv.request.name = strName;
197 
198  if(srvConfigDeleteModbus.call(srv))
199  {
200  //ROS_INFO("receive srv, srv.response.success: %ld\n", (long int)srv.response.success);
201  return (srv.response.success);
202  }
203  else
204  {
205  ROS_ERROR("Failed to call service dr_control_service : config_delete_modbus\n");
206  ros::shutdown();
207  return -1;
208  }
209  return 0;
210 }
211 
213 
214 void thread_subscriber()
215 {
216  ros::NodeHandlePtr node = boost::make_shared<ros::NodeHandle>();
217  ros::MultiThreadedSpinner spinner(2);
218  spinner.spin();
219 }
220 
221 void SigHandler(int sig)
222 {
223  // Do some custom action.
224  // For example, publish a stop message to some other nodes.
225 
226  // All the default sigint handler does is call shutdown()
227  ROS_INFO("shutdown time! sig=%d",sig);
228  ROS_INFO("shutdown time! sig=%d",sig);
229  ROS_INFO("shutdown time! sig=%d",sig);
230  //ros::ServiceClient srvMoveStop = nh.serviceClient<dsr_msgs::MoveStop>("/dsr01m1013/motion/move_stop");
231 
232  ros::NodeHandlePtr node = boost::make_shared<ros::NodeHandle>();
233  ros::Publisher pubRobotStop = node->advertise<dsr_msgs::RobotStop>("/"+ROBOT_ID +ROBOT_MODEL+"/stop",100);
234 
235  dsr_msgs::RobotStop msg;
236 
237  msg.stop_mode = STOP_TYPE_QUICK;
238  pubRobotStop.publish(msg);
239 
240  ros::shutdown();
241 }
242 
243 int main(int argc, char** argv)
244 {
245  //----- set target robot ---------------
246  string my_robot_id = "dsr01";
247  string my_robot_model = "m1013";
248  if(1 == argc){
249  ROS_INFO("default arguments: dsr01 m1013");
250  }
251  else{
252  if(3 != argc){
253  ROS_ERROR("invalid arguments: <ns> <model> (ex) dsr01 m1013");
254  exit(1);
255  }
256  for (int i = 1; i < argc; i++){
257  printf("argv[%d] = %s\n", i, argv[i]);
258  }
259  my_robot_id = argv[1];
260  my_robot_model = argv[2];
261  }
262  //std::cout << "my_robot_id= " << my_robot_id << ", my_robot_model= " << my_robot_model << endl;
263  SET_ROBOT(my_robot_id, my_robot_model);
264 
265  //----- init ROS ----------------------
266  int rate_sub = 1; // 1 Hz = 1 sec
267  int nPubRate = 100; // 100 Hz (10ms)
268  int i=0, nRes=0;
269  ros::init(argc, argv, "dsr_service_modbus_basic_cpp", ros::init_options::NoSigintHandler);
270  ros::NodeHandle nh("~");
271  // Override the default ros sigint handler.
272  // This must be set after the first NodeHandle is created.
273  signal(SIGINT, SigHandler);
274  ros::Publisher pubRobotStop = nh.advertise<dsr_msgs::RobotStop>("/"+ROBOT_ID +ROBOT_MODEL+"/stop",10);
275 
276  // spawn another thread
277  boost::thread thread_sub(thread_subscriber);
278 
279  /*
280  if(argc !=3)
281  {
282  ROS_INFO("cmd : rosrun dsr_control dsr_control_service arg0 arg1");
283  ROS_INFO("arg0: double number, arg1: double number");
284  return 1;
285  }
286  */
287 
289  config_create_modbus("di1", "192.168.137.50", 502, MODBUS_REGISTER_TYPE_DISCRETE_INPUTS, 3);
290  config_create_modbus("do1", "192.168.137.50", 502, MODBUS_REGISTER_TYPE_COILS, 3);
291  config_create_modbus("ro1", "192.168.137.50", 502, MODBUS_REGISTER_TYPE_HOLDING_REGISTER, 7);
292  config_create_modbus("ro2", "192.168.137.50", 502, MODBUS_REGISTER_TYPE_HOLDING_REGISTER, 6);
293 
294  while(ros::ok())
295  {
296  set_modbus_output("ro2", 80);
297  set_modbus_output("do1", 1);
298  ROS_INFO("do1 : %d", get_modbus_input("do1"));
299  ROS_INFO("di1 : %d", get_modbus_input("di1"));
300  if(get_modbus_input("di1") == 1){
301  set_modbus_output("ro1", 30);
302  }
303  }
304 
305  ros::shutdown();
306  // wait the second thread to finish
308  ROS_INFO("dsr_service_test finished !!!!!!!!!!!!!!!!!!!!!");
309  return 0;
310 }
ROBOT_MODEL
string ROBOT_MODEL
Definition: dsr_service_modbus_basic.cpp:70
ros::NodeHandle
ros::init_options::NoSigintHandler
NoSigintHandler
SigHandler
void SigHandler(int sig)
Definition: dsr_service_modbus_basic.cpp:221
config_delete_modbus
int config_delete_modbus(string strName)
Definition: dsr_service_modbus_basic.cpp:190
ros::init
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
ROBOT_ID
string ROBOT_ID
Definition: dsr_service_modbus_basic.cpp:69
movej
int movej(float fTargetPos[NUM_JOINT], float fTargetVel, float fTargetAcc, float fTargetTime=0.f, int nMoveMode=MOVE_MODE_ABSOLUTE, float fBlendingRadius=0.f, int nBlendingType=BLENDING_SPEED_TYPE_DUPLICATE, int nSyncType=0)
Definition: dsr_service_modbus_basic.cpp:74
set_modbus_output
int set_modbus_output(string strName, int nValue)
Definition: dsr_service_modbus_basic.cpp:114
boost::shared_ptr< NodeHandle >
SET_ROBOT
void SET_ROBOT(string id, string model)
Definition: dsr_service_modbus_basic.cpp:71
thread_subscriber
void thread_subscriber()
Definition: dsr_service_modbus_basic.cpp:214
ROS_INFO
#define ROS_INFO(...)
ros::ok
ROSCPP_DECL bool ok()
ros::NodeHandle::advertise
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
main
int main(int argc, char **argv)
Definition: dsr_service_modbus_basic.cpp:243
ros::shutdown
ROSCPP_DECL void shutdown()
config_create_modbus
int config_create_modbus(string strName, string strIP, int nPort, int nRegType, int nRegIndex, int nRegValue=0)
Definition: dsr_service_modbus_basic.cpp:158
get_modbus_input
int get_modbus_input(string strName)
Definition: dsr_service_modbus_basic.cpp:138
ROS_ERROR
#define ROS_ERROR(...)

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