00001 00026 #ifndef SR_EDC_ETHERCAT_DRIVERS_SR_EDC_MUSCLE_H 00027 #define SR_EDC_ETHERCAT_DRIVERS_SR_EDC_MUSCLE_H 00028 00029 #include <ros_ethercat_hardware/ethercat_hardware.h> 00030 #include <sr_edc_ethercat_drivers/sr_edc.h> 00031 #include <realtime_tools/realtime_publisher.h> 00032 #include <std_msgs/Int16.h> 00033 #include <std_msgs/Float64MultiArray.h> 00034 #include <sr_robot_msgs/SimpleMotorFlasher.h> 00035 #include <pthread.h> 00036 #include <bfd.h> 00037 #include <boost/smart_ptr.hpp> 00038 #include <map> 00039 #include <vector> 00040 #include <boost/assign.hpp> 00041 #include <boost/algorithm/string.hpp> 00042 #include <boost/algorithm/string/find_iterator.hpp> 00043 00044 #include <sr_robot_lib/sr_muscle_hand_lib.hpp> 00045 00046 #include <sr_robot_msgs/EthercatDebug.h> 00047 00048 #include <sr_external_dependencies/types_for_external.h> 00049 00050 extern "C" 00051 { 00052 #include <sr_external_dependencies/external/0320_palm_edc_muscle/0320_palm_edc_ethercat_protocol.h> 00053 } 00054 00055 class SrEdcMuscle : 00056 public SrEdc 00057 { 00058 public: 00059 SrEdcMuscle(); 00060 00061 virtual void construct(EtherCAT_SlaveHandler *sh, int &start_address); 00062 00063 virtual int initialize(hardware_interface::HardwareInterface *hw, bool allow_unprogrammed = true); 00064 00065 virtual void multiDiagnostics(vector<diagnostic_msgs::DiagnosticStatus> &vec, unsigned char *buffer); 00066 00067 virtual void packCommand(unsigned char *buffer, bool halt, bool reset); 00068 00069 virtual bool unpackState(unsigned char *this_buffer, unsigned char *prev_buffer); 00070 00071 protected: 00072 typedef realtime_tools::RealtimePublisher<std_msgs::Int16> rt_pub_int16_t; 00073 std::vector<boost::shared_ptr<rt_pub_int16_t> > realtime_pub_; 00074 00076 boost::shared_ptr<realtime_tools::RealtimePublisher<std_msgs::Float64MultiArray> > extra_analog_inputs_publisher; 00077 00079 virtual void reinitialize_boards(); 00080 00089 virtual void get_board_id_and_can_bus(int board_id, int *can_bus, unsigned int *board_can_id); 00090 00091 private: 00092 // std::string firmware_file_name; 00093 00094 // counter for the number of empty buffer we're reading. 00095 unsigned int zero_buffer_read; 00096 00097 boost::shared_ptr<shadow_robot::SrMuscleHandLib<ETHERCAT_DATA_STRUCTURE_0300_PALM_EDC_STATUS, 00098 ETHERCAT_DATA_STRUCTURE_0300_PALM_EDC_COMMAND> > sr_hand_lib; 00099 00104 int16_t cycle_count; 00105 00107 boost::shared_ptr<realtime_tools::RealtimePublisher<sr_robot_msgs::EthercatDebug> > debug_publisher; 00108 }; 00109 00110 00111 /* For the emacs weenies in the crowd. 00112 Local Variables: 00113 c-basic-offset: 2 00114 End: 00115 */ 00116 00117 00118 #endif // SR_EDC_ETHERCAT_DRIVERS_SR_EDC_MUSCLE_H 00119