00001 #ifndef EPOS_HARDWARE_EPOS_H_ 00002 #define EPOS_HARDWARE_EPOS_H_ 00003 00004 #include <ros/ros.h> 00005 #include <hardware_interface/actuator_command_interface.h> 00006 #include <hardware_interface/actuator_state_interface.h> 00007 #include <diagnostic_updater/diagnostic_updater.h> 00008 #include "epos_hardware/utils.h" 00009 00010 namespace epos_hardware { 00011 00012 #define STATUSWORD(b, v) ((v >> b) & 1) 00013 #define READY_TO_SWITCH_ON (0) 00014 #define SWITCHED_ON (1) 00015 #define ENABLE (2) 00016 #define FAULT (3) 00017 #define VOLTAGE_ENABLED (4) 00018 #define QUICKSTOP (5) 00019 #define WARNING (7) 00020 #define TARGET_REACHED (10) 00021 #define CURRENT_LIMIT_ACTIVE (11) 00022 00023 00024 class Epos { 00025 public: 00026 typedef enum { 00027 PROFILE_POSITION_MODE = 1, 00028 PROFILE_VELOCITY_MODE = 3 00029 } OperationMode; 00030 00031 Epos(const std::string& name, 00032 ros::NodeHandle& nh, ros::NodeHandle& config_nh, 00033 EposFactory* epos_factory, 00034 hardware_interface::ActuatorStateInterface& asi, 00035 hardware_interface::VelocityActuatorInterface& avi, 00036 hardware_interface::PositionActuatorInterface& api); 00037 ~Epos(); 00038 bool init(); 00039 void read(); 00040 void write(); 00041 std::string name() { return name_; } 00042 std::string actuator_name() { return actuator_name_; } 00043 void update_diagnostics(); 00044 private: 00045 ros::NodeHandle config_nh_; 00046 diagnostic_updater::Updater diagnostic_updater_; 00047 EposFactory* epos_factory_; 00048 std::string name_; 00049 std::string actuator_name_; 00050 uint64_t serial_number_; 00051 OperationMode operation_mode_; 00052 NodeHandlePtr node_handle_; 00053 bool valid_; 00054 bool has_init_; 00055 00056 double position_; 00057 double velocity_; 00058 double effort_; 00059 double current_; 00060 uint16_t statusword_; 00061 00062 double position_cmd_; 00063 double velocity_cmd_; 00064 int max_profile_velocity_; 00065 bool halt_velocity_; 00066 double torque_constant_; 00067 double nominal_current_; 00068 double max_current_; 00069 00070 void buildMotorStatus(diagnostic_updater::DiagnosticStatusWrapper &stat); 00071 void buildMotorOutputStatus(diagnostic_updater::DiagnosticStatusWrapper &stat); 00072 }; 00073 00074 } 00075 00076 00077 #endif