00001 00025 #ifndef _DENSO_ROBOT_H_ 00026 #define _DENSO_ROBOT_H_ 00027 00028 #include "denso_robot_core/denso_base.h" 00029 #include "denso_robot_core/denso_variable.h" 00030 00031 #define XML_ROBOT_NAME "Robot" 00032 00033 namespace denso_robot_core { 00034 00035 class DensoRobot : public DensoBase 00036 { 00037 public: 00038 DensoRobot(DensoBase* parent, 00039 Service_Vec& service, Handle_Vec& handle, 00040 const std::string& name, const int* mode); 00041 00042 virtual ~DensoRobot(); 00043 00044 virtual HRESULT InitializeBCAP(XMLElement *xmlElem); 00045 00046 virtual HRESULT StartService(ros::NodeHandle& node); 00047 virtual HRESULT StopService(); 00048 00049 virtual bool Update(); 00050 00051 HRESULT get_Variable(const std::string& name, DensoVariable_Ptr* var); 00052 00053 HRESULT AddVariable(const std::string& name); 00054 00055 virtual HRESULT ExecTakeArm() = 0; 00056 virtual HRESULT ExecGiveArm() = 0; 00057 00058 void ChangeArmGroup(int number); 00059 00060 protected: 00061 void Callback_ArmGroup(const Int32::ConstPtr& msg); 00062 00063 virtual HRESULT AddVariable(XMLElement* xmlElem); 00064 00065 HRESULT CreatePoseData( 00066 const PoseData &pose, 00067 VARIANT &vnt); 00068 00069 HRESULT CreateExJoints( 00070 const ExJoints &exjoints, 00071 VARIANT &vnt); 00072 00073 protected: 00074 DensoVariable_Vec m_vecVar; 00075 00076 int32_t m_ArmGroup; 00077 ros::Subscriber m_subArmGroup; 00078 }; 00079 00080 typedef boost::shared_ptr<DensoRobot> DensoRobot_Ptr; 00081 typedef std::vector<DensoRobot_Ptr> DensoRobot_Vec; 00082 00083 } 00084 00085 #endif