00001 00025 #ifndef _DENSO_ROBOT_HW_H_ 00026 #define _DENSO_ROBOT_HW_H_ 00027 00028 #include <ros/ros.h> 00029 00030 // Message (std_msgs) 00031 #include <std_msgs/Int32.h> 00032 #include <std_msgs/Float64MultiArray.h> 00033 using namespace std_msgs; 00034 00035 #include <hardware_interface/joint_command_interface.h> 00036 #include <hardware_interface/joint_state_interface.h> 00037 #include <hardware_interface/robot_hw.h> 00038 00039 #include <joint_limits_interface/joint_limits_interface.h> 00040 00041 #include "denso_robot_core/denso_robot_core.h" 00042 #include "denso_robot_core/denso_controller.h" 00043 #include "denso_robot_core/denso_robot_rc8.h" 00044 #include "denso_robot_core/denso_variable.h" 00045 #include "denso_robot_core/UserIO.h" 00046 using namespace denso_robot_core; 00047 00048 #include <boost/thread.hpp> 00049 00050 #define JOINT_MAX (8) 00051 00052 namespace denso_robot_control 00053 { 00054 class DensoRobotHW : public hardware_interface::RobotHW 00055 { 00056 public: 00057 DensoRobotHW(); 00058 virtual ~DensoRobotHW(); 00059 00060 HRESULT Initialize(); 00061 00062 ros::Time getTime() const 00063 { 00064 return ros::Time::now(); 00065 } 00066 00067 ros::Duration getPeriod() const 00068 { 00069 return ros::Duration(0.008); 00070 } 00071 00072 void read(ros::Time, ros::Duration); 00073 void write(ros::Time, ros::Duration); 00074 00075 private: 00076 HRESULT ChangeModeWithClearError(int mode); 00077 void Callback_ChangeMode(const Int32::ConstPtr& msg); 00078 00079 HRESULT CheckRobotType(); 00080 00081 void Callback_MiniIO(const Int32::ConstPtr& msg); 00082 void Callback_HandIO(const Int32::ConstPtr& msg); 00083 void Callback_SendUserIO(const UserIO::ConstPtr& msg); 00084 void Callback_RecvUserIO(const UserIO::ConstPtr& msg); 00085 00086 private: 00087 hardware_interface::JointStateInterface m_JntStInterface; 00088 hardware_interface::PositionJointInterface m_PosJntInterface; 00089 double m_cmd[JOINT_MAX]; 00090 double m_pos[JOINT_MAX]; 00091 double m_vel[JOINT_MAX]; 00092 double m_eff[JOINT_MAX]; 00093 int m_type[JOINT_MAX]; 00094 std::vector<double> m_joint; 00095 00096 DensoRobotCore_Ptr m_eng; 00097 DensoController_Ptr m_ctrl; 00098 DensoRobotRC8_Ptr m_rob; 00099 DensoVariable_Ptr m_varErr; 00100 00101 std::string m_robName; 00102 int m_robJoints; 00103 int m_sendfmt; 00104 int m_recvfmt; 00105 00106 ros::Subscriber m_subChangeMode; 00107 ros::Subscriber m_subMiniIO; 00108 ros::Subscriber m_subHandIO; 00109 ros::Subscriber m_subSendUserIO; 00110 ros::Subscriber m_subRecvUserIO; 00111 00112 ros::Publisher m_pubCurMode; 00113 ros::Publisher m_pubMiniIO; 00114 ros::Publisher m_pubHandIO; 00115 ros::Publisher m_pubRecvUserIO; 00116 ros::Publisher m_pubCurrent; 00117 00118 boost::mutex m_mtxMode; 00119 }; 00120 00121 } 00122 00123 #endif