00001 // 00002 // Created by tom on 03/04/16. 00003 // 00004 00005 00006 #ifndef ROBOTICAN_HARDWARE_ROBOT_H 00007 #define ROBOTICAN_HARDWARE_ROBOT_H 00008 00009 #include <ros/ros.h> 00010 #include <hardware_interface/joint_command_interface.h> 00011 #include <hardware_interface/joint_state_interface.h> 00012 #include <hardware_interface/robot_hw.h> 00013 #include <robotican_controllers/utils.h> 00014 00015 struct JointInfo_t { 00016 double position; 00017 double effort; 00018 double velocity; 00019 double cmd; 00020 JointInfo_t() { 00021 position = effort = velocity = cmd = 0; 00022 } 00023 }; 00024 00025 class RobotArm : public hardware_interface::RobotHW { 00026 public: 00027 RobotArm(); 00028 ros::Time getTime(); 00029 ros::Duration getPeriod(); 00030 friend std::ostream& operator <<(std::ostream &output, RobotArm& robotArm) { 00031 size_t size = robotArm._arm.size(); 00032 for(std::map<std::string, JointInfo_t>::iterator it = robotArm._arm.begin(); it != robotArm._arm.end(); ++it) 00033 output << " Name: " << it->first << " CMD: " << it->second.cmd << std::endl; 00034 return output; 00035 } 00036 00037 private: 00038 hardware_interface::JointStateInterface _jointStateInterface; 00039 hardware_interface::PositionJointInterface _positionJointInterface; 00040 ros::NodeHandle _nodeHandle; 00041 std::map<std::string, JointInfo_t> _arm; 00042 ros::Time _time; 00043 00044 }; 00045 00046 #endif //ROBOTICAN_HARDWARE_ROBOT_H