dsr_hw_interface.h
Go to the documentation of this file.
1 /*********************************************************************
2  *
3  * Inferfaces for doosan robot controllor
4  * Author: Kab Kyoum Kim (kabkyoum.kim@doosan.com)
5  *
6  * Software License Agreement (BSD License)
7  *
8  * Copyright (c) 2019, Doosan Robotics
9  * All rights reserved.
10  *
11  * Redistribution and use in source and binary forms, with or without
12  * modification, are permitted provided that the following conditions
13  * are met:
14  *
15  * * Redistributions of source code must retain the above copyright
16  * notice, this list of conditions and the following disclaimer.
17  * * Redistributions in binary form must reproduce the above
18  * copyright notice, this list of conditions and the following
19  * disclaimer in the documentation and/or other materials provided
20  * with the distribution.
21  * * Neither the name of the Georgia Institute of Technology nor the names of
22  * its contributors may be used to endorse or promote products derived
23  * from this software without specific prior written permission.
24  *
25  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
26  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
27  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
28  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
29  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
30  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
31  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
32  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
33  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
34  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
35  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
36  * POSSIBILITY OF SUCH DAMAGE.
37  *********************************************************************/
38 
39 #ifndef DR_HW_INTERFACE_H
40 #define DR_HW_INTERFACE_H
41 
42 #include <ros/ros.h>
43 #include <boost/thread/thread.hpp>
44 #include <array>
45 #include <algorithm> // std::copy
46 #include <hardware_interface/joint_command_interface.h>
47 #include <hardware_interface/joint_state_interface.h>
48 #include <hardware_interface/robot_hw.h>
49 
50 #include <std_msgs/Float64MultiArray.h>
51 #include <std_msgs/String.h>
52 #include <std_srvs/Empty.h>
53 
54 // msg
55 #include <dsr_msgs/RobotError.h>
56 #include <dsr_msgs/RobotState.h>
57 #include <dsr_msgs/RobotStop.h>
58 
59 // service
60 //system
61 #include <dsr_msgs/SetRobotMode.h>
62 #include <dsr_msgs/GetRobotMode.h>
63 // motion
64 #include <dsr_msgs/MoveJoint.h>
65 #include <dsr_msgs/MoveLine.h>
66 #include <dsr_msgs/MoveJointx.h>
67 #include <dsr_msgs/MoveCircle.h>
68 #include <dsr_msgs/MoveSplineJoint.h>
69 #include <dsr_msgs/MoveSplineTask.h>
70 #include <dsr_msgs/MoveBlending.h>
71 #include <dsr_msgs/MoveSpiral.h>
72 #include <dsr_msgs/MovePeriodic.h>
73 #include <dsr_msgs/MoveWait.h>
74 //io
75 #include <dsr_msgs/SetCtrlBoxDigitalOutput.h>
76 #include <dsr_msgs/GetCtrlBoxDigitalInput.h>
77 #include <dsr_msgs/SetToolDigitalOutput.h>
78 #include <dsr_msgs/GetToolDigitalInput.h>
79 #include <dsr_msgs/SetCtrlBoxAnalogOutput.h>
80 #include <dsr_msgs/GetCtrlBoxAnalogInput.h>
81 #include <dsr_msgs/SetCtrlBoxAnalogOutputType.h>
82 #include <dsr_msgs/SetCtrlBoxAnalogInputType.h>
83 
84 //modbus
85 #include <dsr_msgs/SetModbusOutput.h>
86 #include <dsr_msgs/GetModbusInput.h>
87 #include <dsr_msgs/ConfigCreateModbus.h>
88 #include <dsr_msgs/ConfigDeleteModbus.h>
89 
90 //drl
91 #include <dsr_msgs/DrlPause.h>
92 #include <dsr_msgs/DrlStart.h>
93 #include <dsr_msgs/DrlStop.h>
94 #include <dsr_msgs/DrlResume.h>
95 #include <dsr_msgs/GetDrlState.h>
96 
97 
98 //tcp
99 #include <dsr_msgs/ConfigCreateTcp.h>
100 #include <dsr_msgs/ConfigDeleteTcp.h>
101 #include <dsr_msgs/GetCurrentTcp.h>
102 #include <dsr_msgs/SetCurrentTcp.h>
103 
104 //tool
105 #include <dsr_msgs/ConfigCreateTool.h>
106 #include <dsr_msgs/ConfigDeleteTool.h>
107 #include <dsr_msgs/GetCurrentTool.h>
108 #include <dsr_msgs/SetCurrentTool.h>
109 
110 //gripper
111 #include <dsr_msgs/Robotiq2FOpen.h>
112 #include <dsr_msgs/Robotiq2FClose.h>
113 #include <dsr_msgs/Robotiq2FMove.h>
114 
115 //serial
116 #include <dsr_msgs/SerialSendData.h>
117 
118 // moveit
119 #include <moveit_msgs/DisplayTrajectory.h>
120 #include <moveit_msgs/RobotTrajectory.h>
121 #include <trajectory_msgs/JointTrajectory.h>
122 #include <trajectory_msgs/JointTrajectoryPoint.h>
123 #include <control_msgs/FollowJointTrajectoryActionGoal.h>
124 
126 #include "../../../common/include/DRFL.h"
127 #include "../../../common/include/dsr_serial.h"
128 
129 #ifndef PI
130 #define PI 3.14159265359
131 #endif
132 #define deg2rad(deg) ((deg) * PI / 180.0)
133 #define rad2deg(rad) ((rad) * 180.0 / PI)
134 
135 typedef struct {
136  int nLevel; // INFO =1, WARN =2, ERROR =3
137  int nGroup; // SYSTEM =1, MOTION =2, TP =3, INVERTER =4, SAFETY_CONTROLLER =5
138  int nCode; // error code
139  char strMsg1[MAX_STRING_SIZE]; // error msg 1
140  char strMsg2[MAX_STRING_SIZE]; // error msg 2
141  char strMsg3[MAX_STRING_SIZE]; // error msg 3
142 } DR_ERROR, *LPDR_ERROR;
143 
144 typedef struct {
145  int nRobotState;
146  char strRobotState[MAX_SYMBOL_SIZE];
147  float fCurrentPosj[NUM_JOINT];
148  float fCurrentPosx[NUM_TASK];
149 
150  int nActualMode;
151  int nActualSpace;
152 
153  float fJointAbs[NUM_JOINT];
154  float fJointErr[NUM_JOINT];
155  float fTargetPosj[NUM_JOINT];
156  float fTargetVelj[NUM_JOINT];
157  float fCurrentVelj[NUM_JOINT];
158 
159  float fTaskErr[NUM_TASK];
160  float fTargetPosx[NUM_TASK];
161  float fTargetVelx[NUM_TASK];
162  float fCurrentVelx[NUM_TASK];
163  int nSolutionSpace;
164  float fRotationMatrix[3][3];
165 
166  float fDynamicTor[NUM_JOINT];
167  float fActualJTS[NUM_JOINT];
168  float fActualEJT[NUM_JOINT];
169  float fActualETT[NUM_JOINT];
170 
171  double dSyncTime;
172  int nActualBK[NUM_JOINT];
173  int nActualBT[NUM_BUTTON];
174  float fActualMC[NUM_JOINT];
175  float fActualMT[NUM_JOINT];
176  bool bCtrlBoxDigitalOutput[16];
177  bool bCtrlBoxDigitalInput[16];
178  bool bFlangeDigitalOutput[6];
179  bool bFlangeDigitalInput[6];
180 
181  //io_modbus # GJH
182  int nRegCount;
183  string strModbusSymbol[100];
184  int nModbusValue[100];
185  //error # GJH
186  int nAccessControl;
187  bool bHommingCompleted;
188  bool bTpInitialized;
189  bool bMasteringNeed;
190  bool bDrlStopped;
191  bool bDisconnected;
192 } DR_STATE, *LPDR_STATE;
193 
194 using namespace DRAFramework;
195 
196 namespace dsr_control{
197 
198  class DRHWInterface : public hardware_interface::RobotHW
199  {
200  public:
201  DRHWInterface(ros::NodeHandle& nh);
202  virtual ~DRHWInterface();
203 
204  bool init();
205  virtual void read(ros::Duration& elapsed_time);
206  virtual void write(ros::Duration& elapsed_time);
207  int MsgPublisher_RobotState();
209  static void OnHommingCompletedCB();
210  static void OnProgramStoppedCB(const PROGRAM_STOP_CAUSE iStopCause);
211  static void OnMonitoringDataCB(const LPMONITORING_DATA pData);
212  static void OnTpInitializingCompletedCB();
213 
214  static void OnMonitoringCtrlIOCB (const LPMONITORING_CTRLIO pCtrlIO);
215  static void OnMonitoringModbusCB (const LPMONITORING_MODBUS pModbus);
216  static void OnMonitoringStateCB(const ROBOT_STATE eState);
217  static void OnMonitoringAccessControlCB(const MONITORING_ACCESS_CONTROL eAccCtrl);
218  static void OnLogAlarm(LPLOG_ALARM pLogAlarm);
219 
220 
221  private:
222  ros::NodeHandle private_nh_;
223 
224  std::string m_strRobotName;
225  std::string m_strRobotModel;
226  std::string m_strRobotGripper;
227 
228  //----- Service ---------------------------------------------------------------
229  ros::ServiceServer m_nh_system[4];
230  ros::ServiceServer m_nh_move_service[10];
231  ros::ServiceServer m_nh_io_service[8];
232  ros::ServiceServer m_nh_modbus_service[4];
233  ros::ServiceServer m_nh_drl_service[10];
234  ros::ServiceServer m_nh_tcp_service[4];
235  ros::ServiceServer m_nh_tool_service[4];
236  ros::ServiceServer m_nh_gripper_service[10];
237  ros::ServiceServer m_nh_serial_service[4];
238 
239  //----- Publisher -------------------------------------------------------------
240  ros::Publisher m_PubRobotState;
241  ros::Publisher m_PubRobotError;
242  ros::Publisher m_PubtoGazebo;
243  ros::Publisher m_PubSerialWrite;
244 
245  //----- Subscriber ------------------------------------------------------------
246  ros::Subscriber m_sub_joint_trajectory;
247  ros::Subscriber m_sub_joint_position;
248  ros::Subscriber m_SubSerialRead;
249 
250  // ROS Interface
251  hardware_interface::JointStateInterface jnt_state_interface;
252  hardware_interface::PositionJointInterface jnt_pos_interface;
253  hardware_interface::VelocityJointInterface velocity_joint_interface_;
254 
255  std::array<float, NUM_JOINT> cmd_;
256  bool bCommand_;
257  struct Joint{
258  double cmd;
259  double pos;
260  double vel;
261  double eff;
262  Joint(): cmd(0), pos(0), vel(0), eff(0) {}
263  } joints[NUM_JOINT];
264 
265  //----- SIG Handler --------------------------------------------------------------
266  void sigint_handler( int signo);
267  void trajectoryCallback(const control_msgs::FollowJointTrajectoryActionGoal::ConstPtr& msg);
268  void positionCallback(const std_msgs::Float64MultiArray::ConstPtr& msg);
269 
270  //----- Threads ------------------------------------------------------------------
271  boost::thread m_th_subscribe; //subscribe thread
272  boost::thread m_th_publisher; //publisher thread
273  static void thread_subscribe(ros::NodeHandle nh);
274  static void thread_publisher(DRHWInterface* pDRHWInterface, ros::NodeHandle nh, int nPubRate);
275 
276  DR_STATE m_stDrState;
277  DR_ERROR m_stDrError;
278 
279  //----- Service Call-back functions ----------------------------------------------.
280  //----- System
281  bool set_robot_mode_cb(dsr_msgs::SetRobotMode::Request& req, dsr_msgs::SetRobotMode::Response& res);
282  bool get_robot_mode_cb(dsr_msgs::GetRobotMode::Request& req, dsr_msgs::GetRobotMode::Response& res);
283  //----- MOTION
284  bool movej_cb(dsr_msgs::MoveJoint::Request& req, dsr_msgs::MoveJoint::Response& res);
285  bool movel_cb(dsr_msgs::MoveLine::Request& req, dsr_msgs::MoveLine::Response& res);
286  bool movejx_cb(dsr_msgs::MoveJointx::Request& req, dsr_msgs::MoveJointx::Response& res);
287  bool movec_cb(dsr_msgs::MoveCircle::Request& req, dsr_msgs::MoveCircle::Response& res);
288  bool movesj_cb(dsr_msgs::MoveSplineJoint::Request& req, dsr_msgs::MoveSplineJoint::Response& res);
289  bool movesx_cb(dsr_msgs::MoveSplineTask::Request& req, dsr_msgs::MoveSplineTask::Response& res);
290  bool moveb_cb(dsr_msgs::MoveBlending::Request& req, dsr_msgs::MoveBlending::Response& res);
291  bool movespiral_cb(dsr_msgs::MoveSpiral::Request& req, dsr_msgs::MoveSpiral::Response& res);
292  bool moveperiodic_cb(dsr_msgs::MovePeriodic::Request& req, dsr_msgs::MovePeriodic::Response& res);
293  bool movewait_cb(dsr_msgs::MoveWait::Request& req, dsr_msgs::MoveWait::Response& res);
294 
295 
296  //----- TCP
297  bool set_current_tcp_cb(dsr_msgs::SetCurrentTcp::Request& req, dsr_msgs::SetCurrentTcp::Response& res);
298  bool get_current_tcp_cb(dsr_msgs::GetCurrentTcp::Request& req, dsr_msgs::GetCurrentTcp::Response& res);
299  bool config_create_tcp_cb(dsr_msgs::ConfigCreateTcp::Request& req, dsr_msgs::ConfigCreateTcp::Response& res);
300  bool config_delete_tcp_cb(dsr_msgs::ConfigDeleteTcp::Request& req, dsr_msgs::ConfigDeleteTcp::Response& res);
301 
302  //----- TOOL
303  bool set_current_tool_cb(dsr_msgs::SetCurrentTool::Request& req, dsr_msgs::SetCurrentTool::Response& res);
304  bool get_current_tool_cb(dsr_msgs::GetCurrentTool::Request& req, dsr_msgs::GetCurrentTool::Response& res);
305  bool config_create_tool_cb(dsr_msgs::ConfigCreateTool::Request& req, dsr_msgs::ConfigCreateTool::Response& res);
306  bool config_delete_tool_cb(dsr_msgs::ConfigDeleteTool::Request& req, dsr_msgs::ConfigDeleteTool::Response& res);
307 
308  //----- IO
309  bool set_digital_output_cb(dsr_msgs::SetCtrlBoxDigitalOutput::Request& req, dsr_msgs::SetCtrlBoxDigitalOutput::Response& res);
310  bool get_digital_input_cb(dsr_msgs::GetCtrlBoxDigitalInput::Request& req, dsr_msgs::GetCtrlBoxDigitalInput::Response& res);
311  bool set_tool_digital_output_cb(dsr_msgs::SetToolDigitalOutput::Request& req, dsr_msgs::SetToolDigitalOutput::Response& res);
312  bool get_tool_digital_input_cb(dsr_msgs::GetToolDigitalInput::Request& req, dsr_msgs::GetToolDigitalInput::Response& res);
313 
314  bool set_analog_output_cb(dsr_msgs::SetCtrlBoxAnalogOutput::Request& req, dsr_msgs::SetCtrlBoxAnalogOutput::Response& res);
315  bool get_analog_input_cb(dsr_msgs::GetCtrlBoxAnalogInput::Request& req, dsr_msgs::GetCtrlBoxAnalogInput::Response& res);
316  bool set_analog_output_type_cb(dsr_msgs::SetCtrlBoxAnalogOutputType::Request& req, dsr_msgs::SetCtrlBoxAnalogOutputType::Response& res);
317  bool set_analog_input_type_cb(dsr_msgs::SetCtrlBoxAnalogInputType::Request& req, dsr_msgs::SetCtrlBoxAnalogInputType::Response& res);
318 
319  //----- MODBUS
320  bool set_modbus_output_cb(dsr_msgs::SetModbusOutput::Request& req, dsr_msgs::SetModbusOutput::Response& res);
321  bool get_modbus_input_cb(dsr_msgs::GetModbusInput::Request& req, dsr_msgs::GetModbusInput::Response& res);
322  bool config_create_modbus_cb(dsr_msgs::ConfigCreateModbus::Request& req, dsr_msgs::ConfigCreateModbus::Response& res);
323  bool config_delete_modbus_cb(dsr_msgs::ConfigDeleteModbus::Request& req, dsr_msgs::ConfigDeleteModbus::Response& res);
324 
325  //----- DRL
326  bool drl_pause_cb(dsr_msgs::DrlPause::Request& req, dsr_msgs::DrlPause::Response& res);
327  bool drl_start_cb(dsr_msgs::DrlStart::Request& req, dsr_msgs::DrlStart::Response& res);
328  bool drl_stop_cb(dsr_msgs::DrlStop::Request& req, dsr_msgs::DrlStop::Response& res);
329  bool drl_resume_cb(dsr_msgs::DrlResume::Request& req, dsr_msgs::DrlResume::Response& res);
330  bool get_drl_state_cb(dsr_msgs::GetDrlState::Request& req, dsr_msgs::GetDrlState::Response& res);
331 
332  //----- Gripper
333  bool robotiq_2f_open_cb(dsr_msgs::Robotiq2FOpen::Request& req, dsr_msgs::Robotiq2FOpen::Response& res);
334  bool robotiq_2f_close_cb(dsr_msgs::Robotiq2FClose::Request& req, dsr_msgs::Robotiq2FClose::Response& res);
335  bool robotiq_2f_move_cb(dsr_msgs::Robotiq2FMove::Request& req, dsr_msgs::Robotiq2FMove::Response& res);
336 
337  //----- Serial
338  bool serial_send_data_cb(dsr_msgs::SerialSendData::Request& req, dsr_msgs::SerialSendData::Response& res);
339 
340  };
341 }
342 
343 #endif // end
dsr_control::DRHWInterface::jnt_state_interface
hardware_interface::JointStateInterface jnt_state_interface
Definition: dsr_hw_interface.h:251
DR_STATE::nActualSpace
int nActualSpace
Definition: dsr_hw_interface.h:151
hardware_interface::JointStateInterface
DR_STATE::bHommingCompleted
bool bHommingCompleted
Definition: dsr_hw_interface.h:187
ros::NodeHandle
dsr_control::DRHWInterface::m_strRobotModel
std::string m_strRobotModel
Definition: dsr_hw_interface.h:225
DR_STATE::nRobotState
int nRobotState
Definition: dsr_hw_interface.h:145
DR_ERROR
Definition: dsr_hw_interface.h:135
dsr_control::DRHWInterface::m_PubRobotError
ros::Publisher m_PubRobotError
Definition: dsr_hw_interface.h:241
dsr_control::DRHWInterface::m_stDrError
DR_ERROR m_stDrError
Definition: dsr_hw_interface.h:277
LPDR_ERROR
struct DR_ERROR * LPDR_ERROR
init
void init(const M_string &remappings)
dsr_control::DRHWInterface::Joint::pos
double pos
Definition: dsr_hw_interface.h:259
dsr_control::DRHWInterface::Joint::Joint
Joint()
Definition: dsr_hw_interface.h:262
dsr_control::DRHWInterface::m_stDrState
DR_STATE m_stDrState
Definition: dsr_hw_interface.h:276
dsr_control::DRHWInterface::m_th_subscribe
boost::thread m_th_subscribe
Definition: dsr_hw_interface.h:271
dsr_control::DRHWInterface::m_strRobotName
std::string m_strRobotName
Definition: dsr_hw_interface.h:224
robot_hw.h
DR_ERROR::nCode
int nCode
Definition: dsr_hw_interface.h:138
joint_state_interface.h
dsr_control::DRHWInterface::bCommand_
bool bCommand_
Definition: dsr_hw_interface.h:256
DR_STATE::bMasteringNeed
bool bMasteringNeed
Definition: dsr_hw_interface.h:189
LPDR_STATE
struct DR_STATE * LPDR_STATE
hardware_interface::VelocityJointInterface
dsr_control::DRHWInterface::Joint
Definition: dsr_hw_interface.h:257
dsr_control::DRHWInterface::Joint::vel
double vel
Definition: dsr_hw_interface.h:260
dsr_control::DRHWInterface::private_nh_
ros::NodeHandle private_nh_
Definition: dsr_hw_interface.h:222
DR_ERROR::nLevel
int nLevel
Definition: dsr_hw_interface.h:136
DR_STATE::bDrlStopped
bool bDrlStopped
Definition: dsr_hw_interface.h:190
dsr_control::DRHWInterface::m_PubRobotState
ros::Publisher m_PubRobotState
Definition: dsr_hw_interface.h:240
DR_STATE::bDisconnected
bool bDisconnected
Definition: dsr_hw_interface.h:191
DR_STATE::nActualMode
int nActualMode
Definition: dsr_hw_interface.h:150
dsr_control::DRHWInterface::jnt_pos_interface
hardware_interface::PositionJointInterface jnt_pos_interface
Definition: dsr_hw_interface.h:252
dsr_control::DRHWInterface::Joint::eff
double eff
Definition: dsr_hw_interface.h:261
dsr_control::DRHWInterface::m_sub_joint_trajectory
ros::Subscriber m_sub_joint_trajectory
Definition: dsr_hw_interface.h:246
dsr_control::DRHWInterface::Joint::cmd
double cmd
Definition: dsr_hw_interface.h:258
DR_ERROR::nGroup
int nGroup
Definition: dsr_hw_interface.h:137
dsr_control::DRHWInterface::m_strRobotGripper
std::string m_strRobotGripper
Definition: dsr_hw_interface.h:226
dsr_control::DRHWInterface::velocity_joint_interface_
hardware_interface::VelocityJointInterface velocity_joint_interface_
Definition: dsr_hw_interface.h:253
DR_STATE::nSolutionSpace
int nSolutionSpace
Definition: dsr_hw_interface.h:163
DR_STATE::nAccessControl
int nAccessControl
Definition: dsr_hw_interface.h:186
joint_command_interface.h
DR_STATE::bTpInitialized
bool bTpInitialized
Definition: dsr_hw_interface.h:188
dsr_control::DRHWInterface::m_PubtoGazebo
ros::Publisher m_PubtoGazebo
Definition: dsr_hw_interface.h:242
dsr_control
Definition: dsr_hw_interface.h:196
dsr_control::DRHWInterface::m_PubSerialWrite
ros::Publisher m_PubSerialWrite
Definition: dsr_hw_interface.h:243
dsr_control::DRHWInterface::m_sub_joint_position
ros::Subscriber m_sub_joint_position
Definition: dsr_hw_interface.h:247
DR_STATE
Definition: dsr_hw_interface.h:144
dsr_control::DRHWInterface::cmd_
std::array< float, NUM_JOINT > cmd_
Definition: dsr_hw_interface.h:255
DR_STATE::nRegCount
int nRegCount
Definition: dsr_hw_interface.h:182
dsr_control::DRHWInterface::m_SubSerialRead
ros::Subscriber m_SubSerialRead
Definition: dsr_hw_interface.h:248
hardware_interface::RobotHW
dsr_control::DRHWInterface::m_th_publisher
boost::thread m_th_publisher
Definition: dsr_hw_interface.h:272
dsr_control::DRHWInterface
Definition: dsr_hw_interface.h:198
hardware_interface::PositionJointInterface
DR_STATE::dSyncTime
double dSyncTime
Definition: dsr_hw_interface.h:171

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