00001 00028 #ifndef GENERIC_TACTILES_HPP_ 00029 #define GENERIC_TACTILES_HPP_ 00030 00031 #include <boost/smart_ptr.hpp> 00032 #include <sr_external_dependencies/types_for_external.h> 00033 00034 extern "C" 00035 { 00036 #include <sr_external_dependencies/external/0220_palm_edc/0220_palm_edc_ethercat_protocol.h> 00037 #include <sr_external_dependencies/external/0230_palm_edc_TS/0230_palm_edc_ethercat_protocol.h> 00038 #include <sr_external_dependencies/external/0320_palm_edc_muscle/0320_palm_edc_ethercat_protocol.h> 00039 } 00040 00041 #include <ros/ros.h> 00042 #include <vector> 00043 #include <string> 00044 #include <std_srvs/Empty.h> 00045 00046 #include <diagnostic_msgs/DiagnosticStatus.h> 00047 #include <diagnostic_updater/DiagnosticStatusWrapper.h> 00048 00049 #include <sr_hardware_interface/tactile_sensors.hpp> 00050 #include "sr_robot_lib/generic_updater.hpp" 00051 #include "sr_robot_lib/sensor_updater.hpp" 00052 00053 namespace tactiles 00054 { 00055 template<class StatusType, class CommandType> 00056 class GenericTactiles 00057 { 00058 public: 00059 GenericTactiles(ros::NodeHandle nh, std::string device_id, 00060 std::vector<generic_updater::UpdateConfig> update_configs_vector, 00061 operation_mode::device_update_state::DeviceUpdateState update_state); 00062 00063 virtual ~GenericTactiles() 00064 { 00065 } 00066 00074 virtual void update(StatusType *status_data); 00075 00080 virtual void publish(); 00081 00086 virtual void add_diagnostics(std::vector<diagnostic_msgs::DiagnosticStatus> &vec, 00087 diagnostic_updater::DiagnosticStatusWrapper &d); 00088 00097 bool reset(std_srvs::Empty::Request &request, 00098 std_srvs::Empty::Response &response); 00099 00101 static const unsigned int nb_tactiles; 00102 00103 boost::shared_ptr<generic_updater::SensorUpdater<CommandType> > sensor_updater; 00105 boost::shared_ptr<std::vector<GenericTactileData> > tactiles_vector; 00106 00107 virtual std::vector<AllTactileData> *get_tactile_data(); 00108 00109 protected: 00110 void process_received_data_type(int32u data); 00111 00112 ros::NodeHandle nodehandle_; 00113 std::string device_id_; 00114 00115 ros::ServiceServer reset_service_client_; 00116 00117 // Contains the received data types. 00118 std::vector<int32u> initialization_received_data_vector; 00119 00130 std::string sanitise_string(const char *raw_string, const unsigned int str_size); 00131 00132 boost::shared_ptr<std::vector<AllTactileData> > all_tactile_data; 00133 }; // end class 00134 } // namespace tactiles 00135 00136 /* For the emacs weenies in the crowd. 00137 Local Variables: 00138 c-basic-offset: 2 00139 End: 00140 */ 00141 00142 #endif /* GENERIC_TACTILES_HPP_ */