dsr_service_drl_simple.cpp
Go to the documentation of this file.
1 /*
2  * [c++ example simple] DRL(Doosan Robot Language) 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 <sstream>
12 #include <std_msgs/String.h>
13 
14 #include "dsr_util.h"
15 #include "dsr_robot.h"
16 
17 using namespace std;
18 using namespace DSR_Robot;
19 
20 std::string temp_data;
21 //----- set tartget robot----------------------------------------------------
22 string ROBOT_ID = "dsr01";
23 string ROBOT_MODEL = "m1013";
24 void SET_ROBOT(string id, string model) {ROBOT_ID = id; ROBOT_MODEL= model;}
25 //---------------------------------------------------------------------------
26 
28 void msgRobotState_cb(const dsr_msgs::RobotState::ConstPtr& msg)
29 {
30  static int sn_cnt =0;
31 
32  sn_cnt++;
33  if(0==(sn_cnt % 100))
34  {
35  ROS_INFO("________ ROBOT STATUS ________");
36  ROS_INFO(" robot_state : %d", msg->robot_state);
37  ROS_INFO(" robot_state_str : %s", msg->robot_state_str.c_str());
38  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]
39  ,msg->current_posj[3] ,msg->current_posj[4] ,msg->current_posj[5] );
40  ROS_INFO(" current_posx : %7.3f %7.3f %7.3f %7.3f %7.3f %7.3f",msg->current_posx[0] ,msg->current_posx[1] ,msg->current_posx[2]
41  ,msg->current_posx[3] ,msg->current_posx[4] ,msg->current_posx[5] );
42  temp_data = "";
43  for(int i = 0; i < 16; i++){
44  temp_data += (std::to_string(msg->ctrlbox_digital_input[i]) + " ");
45  }
46  ROS_INFO(" ctrlbox_digital_input : %s", temp_data.c_str());
47 
48  temp_data = "";
49  for(int i = 0; i < 16; i++){
50  temp_data += (std::to_string(msg->ctrlbox_digital_output[i]) + " ");
51  }
52  ROS_INFO(" ctrlbox_digital_output : %s", temp_data.c_str());
53 
54  temp_data = "";
55  for(int i = 0; i < 6; i++){
56  temp_data += (std::to_string(msg->flange_digital_input[i]) + " ");
57  }
58  ROS_INFO(" flange_digital_input : %s", temp_data.c_str());
59 
60  temp_data = "";
61  for(int i = 0; i < 6; i++){
62  temp_data += (std::to_string(msg->flange_digital_output[i]) + " ");
63  }
64  ROS_INFO(" flange_digital_output : %s", temp_data.c_str());
65 
66  temp_data = "";
67  for(int i = 0; i < msg->modbus_state.size(); i++){
68  temp_data += ("[" + msg->modbus_state[i].modbus_symbol + " , " + std::to_string(msg->modbus_state[i].modbus_value) + "] ");
69  }
70  ROS_INFO(" modbus_state : %s", temp_data.c_str());
71  ROS_INFO(" access_control : %d", msg->access_control);
72  ROS_INFO(" homming_completed : %d", msg->homming_completed);
73  ROS_INFO(" tp_initialized : %d", msg->tp_initialized);
74  ROS_INFO(" joint_speed : %7.3f %7.3f %7.3f %7.3f %7.3f %7.3f",msg->current_velj[0] ,msg->current_velj[1] ,msg->current_velj[2]
75  ,msg->current_velj[3] ,msg->current_velj[4] ,msg->current_velj[5] );
76  ROS_INFO(" task_speed : %7.3f %7.3f %7.3f %7.3f %7.3f %7.3f",msg->current_velx[0] ,msg->current_velx[1] ,msg->current_velx[2]
77  ,msg->current_velx[3] ,msg->current_velx[4] ,msg->current_velx[5] );
78  ROS_INFO(" mastering_need : %d", msg->mastering_need);
79  ROS_INFO(" drl_stopped : %d", msg->drl_stopped);
80  ROS_INFO(" disconnected : %d", msg->disconnected);
81  }
82 }
83 
84 static void thread_subscriber()
85 {
86  ros::NodeHandlePtr node = boost::make_shared<ros::NodeHandle>();
89  ros::MultiThreadedSpinner spinner(2);
90  spinner.spin();
91 }
92 
93 void SigHandler(int sig)
94 {
95  // Do some custom action.
96  // For example, publish a stop message to some other nodes.
97 
98  // All the default sigint handler does is call shutdown()
99  ROS_INFO("shutdown time!");
100  ROS_INFO("shutdown time!");
101  ROS_INFO("shutdown time!");
102  //ros::ServiceClient srvMoveStop = nh.serviceClient<dsr_msgs::MoveStop>("/dsr01m1013/motion/move_stop");
103 
104  ros::NodeHandlePtr node = boost::make_shared<ros::NodeHandle>();
105  ros::Publisher pubRobotStop = node->advertise<dsr_msgs::RobotStop>("/"+ROBOT_ID +ROBOT_MODEL+"/stop",100);
106 
107  dsr_msgs::RobotStop msg;
108 
109  msg.stop_mode = STOP_TYPE_QUICK;
110  pubRobotStop.publish(msg);
111 
112  ros::shutdown();
113 }
114 
115 int main(int argc, char** argv)
116 {
117  //----- set target robot ---------------
118  string my_robot_id = "dsr01";
119  string my_robot_model = "m1013";
120  if(1 == argc){
121  ROS_INFO("default arguments: dsr01 m1013");
122  }
123  else{
124  if(3 != argc){
125  ROS_ERROR("invalid arguments: <ns> <model> (ex) dsr01 m1013");
126  exit(1);
127  }
128  for (int i = 1; i < argc; i++){
129  printf("argv[%d] = %s\n", i, argv[i]);
130  }
131  my_robot_id = argv[1];
132  my_robot_model = argv[2];
133  }
134  //std::cout << "my_robot_id= " << my_robot_id << ", my_robot_model= " << my_robot_model << endl;
135  SET_ROBOT(my_robot_id, my_robot_model);
136 
137  //----- init ROS ----------------------
138  ros::init(argc, argv, "dsr_service_drl_cpp", ros::init_options::NoSigintHandler);
139  ros::NodeHandle nh("~");
140  // Override the default ros sigint handler.
141  // This must be set after the first NodeHandle is created.
142  signal(SIGINT, SigHandler);
143  ros::Publisher pubRobotStop = nh.advertise<dsr_msgs::RobotStop>("/"+ROBOT_ID +ROBOT_MODEL+"/stop",10);
145 
146  //----- create DsrRobot ---------------
147  CDsrRobot robot(nh, my_robot_id, my_robot_model);
148 
149  // run subscriber thread (for monitoring)
150  boost::thread thread_sub(thread_subscriber);
151 
153  std::string drlCodeMove = "set_velj(50)\nset_accj(50)\nmovej([0,0,90,0,90,0])\n";
154  std::string drlCodeReset = "movej([0,0,0,0,0,0])\n";
155  std::string mode;
156  int _rate;
157  nh.getParam("/dsr/mode", mode);
158  ROS_INFO("current mode is : %s", mode.c_str());
159  if(mode == "real"){
160  robot.drl_start(ROBOT_SYSTEM_REAL, drlCodeMove + drlCodeReset);
161  }
162  else{
163  robot.drl_start(ROBOT_SYSTEM_VIRTUAL, drlCodeMove + drlCodeReset);
164  }
165  //robot.drl_start(ROBOT_SYSTEM_VIRTUAL, drlCodeReset);
166 
167  while(ros::ok())
168  {
169  }
170  thread_sub.join();
171  ROS_INFO("dsr_service_test finished !!!!!!!!!!!!!!!!!!!!!");
172  return 0;
173 }
msgRobotState_cb
void msgRobotState_cb(const dsr_msgs::RobotState::ConstPtr &msg)
Definition: dsr_service_drl_simple.cpp:28
main
int main(int argc, char **argv)
Definition: dsr_service_drl_simple.cpp:115
ros::NodeHandle
ros::init_options::NoSigintHandler
NoSigintHandler
ros::init
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
DSR_Robot
SigHandler
void SigHandler(int sig)
Definition: dsr_service_drl_simple.cpp:93
boost::shared_ptr< NodeHandle >
thread_subscriber
static void thread_subscriber()
Definition: dsr_service_drl_simple.cpp:84
ROBOT_MODEL
string ROBOT_MODEL
Definition: dsr_service_drl_simple.cpp:23
ROS_INFO
#define ROS_INFO(...)
temp_data
std::string temp_data
Definition: dsr_service_drl_simple.cpp:20
ros::ok
ROSCPP_DECL bool ok()
ros::NodeHandle::advertise
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
ros::NodeHandle::getParam
bool getParam(const std::string &key, std::string &s) const
SET_ROBOT
void SET_ROBOT(string id, string model)
Definition: dsr_service_drl_simple.cpp:24
ros::shutdown
ROSCPP_DECL void shutdown()
ROS_ERROR
#define ROS_ERROR(...)
ROBOT_ID
string ROBOT_ID
Definition: dsr_service_drl_simple.cpp:22

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