00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035 #ifndef ETHERCAT_HARDWARE_H
00036 #define ETHERCAT_HARDWARE_H
00037
00038 #include <pr2_hardware_interface/hardware_interface.h>
00039
00040 #include <al/ethercat_AL.h>
00041 #include <al/ethercat_master.h>
00042 #include <al/ethercat_slave_handler.h>
00043
00044 #include "ethercat_hardware/ethercat_device.h"
00045 #include "ethercat_hardware/ethercat_com.h"
00046 #include "ethercat_hardware/ethernet_interface_info.h"
00047
00048 #include <realtime_tools/realtime_publisher.h>
00049
00050 #include <boost/accumulators/accumulators.hpp>
00051 #include <boost/accumulators/statistics/stats.hpp>
00052 #include <boost/accumulators/statistics/max.hpp>
00053 #include <boost/accumulators/statistics/mean.hpp>
00054
00055 #include <boost/thread/thread.hpp>
00056 #include <boost/thread/mutex.hpp>
00057 #include <boost/thread/condition_variable.hpp>
00058
00059 #include <pluginlib/class_loader.h>
00060
00061 #include <std_msgs/Bool.h>
00062
00063 using namespace boost::accumulators;
00064
00065 struct EthercatHardwareDiagnostics
00066 {
00067 EthercatHardwareDiagnostics();
00068 void resetMaxTiming();
00069 accumulator_set<double, stats<tag::max, tag::mean> > pack_command_acc_;
00070 accumulator_set<double, stats<tag::max, tag::mean> > txandrx_acc_;
00071 accumulator_set<double, stats<tag::max, tag::mean> > unpack_state_acc_;
00072 accumulator_set<double, stats<tag::max, tag::mean> > publish_acc_;
00073 double max_pack_command_;
00074 double max_txandrx_;
00075 double max_unpack_state_;
00076 double max_publish_;
00077 int txandrx_errors_;
00078 unsigned device_count_;
00079 bool pd_error_;
00080 bool halt_after_reset_;
00081 unsigned reset_motors_service_count_;
00082 unsigned halt_motors_service_count_;
00083 unsigned halt_motors_error_count_;
00084 struct netif_counters counters_;
00085 bool input_thread_is_stopped_;
00086 bool motors_halted_;
00087
00088 static const bool collect_extra_timing_ = true;
00089 };
00090
00091
00105 class EthercatHardwareDiagnosticsPublisher
00106 {
00107 public:
00108
00109 EthercatHardwareDiagnosticsPublisher(ros::NodeHandle &node);
00110 ~EthercatHardwareDiagnosticsPublisher();
00111
00117 void initialize(const string &interface, unsigned int buffer_size, EthercatDevice **slaves, unsigned int num_slaves,
00118 unsigned timeout, unsigned max_pd_retries);
00119
00127 void publish(const unsigned char *buffer, const EthercatHardwareDiagnostics &diagnostics);
00128
00132 void stop();
00133
00134 private:
00135
00143 void publishDiagnostics();
00144
00150 void diagnosticsThreadFunc();
00151
00152
00156 static void timingInformation(
00157 diagnostic_updater::DiagnosticStatusWrapper &status,
00158 const string &key,
00159 const accumulator_set<double, stats<tag::max, tag::mean> > &acc,
00160 double max);
00161
00162 ros::NodeHandle node_;
00163
00164 boost::mutex diagnostics_mutex_;
00165 boost::condition_variable diagnostics_cond_;
00166 bool diagnostics_ready_;
00167 boost::thread diagnostics_thread_;
00168
00169 ros::Publisher publisher_;
00170
00171 EthercatHardwareDiagnostics diagnostics_;
00172 unsigned char *diagnostics_buffer_;
00173 unsigned int buffer_size_;
00174 EthercatDevice **slaves_;
00175 unsigned int num_slaves_;
00176 string interface_;
00177
00179 unsigned timeout_;
00181 unsigned max_pd_retries_;
00182
00184 uint64_t last_dropped_packet_count_;
00186 ros::Time last_dropped_packet_time_;
00188 static const unsigned dropped_packet_warning_hold_time_ = 10;
00189
00190 diagnostic_msgs::DiagnosticArray diagnostic_array_;
00192 EthernetInterfaceInfo ethernet_interface_info_;
00193 vector<diagnostic_msgs::KeyValue> values_;
00194 diagnostic_updater::DiagnosticStatusWrapper status_;
00195 };
00196
00197
00198 class EthercatHardware
00199 {
00200 public:
00204 EthercatHardware(const std::string& name);
00205
00209 ~EthercatHardware();
00210
00216 void update(bool reset, bool halt);
00217
00223 void init(char *interface, bool allow_unprogrammed);
00224
00228 void collectDiagnostics();
00229
00230 void printCounters(std::ostream &os=std::cout);
00231
00235 bool txandrx_PD(unsigned buffer_size, unsigned char* buffer, unsigned tries);
00236
00246 bool publishTrace(int position, const string &reason, unsigned level, unsigned delay);
00247
00248 pr2_hardware_interface::HardwareInterface *hw_;
00249
00250 private:
00251 static void changeState(EtherCAT_SlaveHandler *sh, EC_State new_state);
00252
00253 ros::NodeHandle node_;
00254
00255 struct netif *ni_;
00256 string interface_;
00257
00258 EtherCAT_AL *al_;
00259 EtherCAT_Master *em_;
00260
00261 EthercatDevice *configSlave(EtherCAT_SlaveHandler *sh);
00262 EthercatDevice **slaves_;
00263 unsigned int num_slaves_;
00264
00265 unsigned char *this_buffer_;
00266 unsigned char *prev_buffer_;
00267 unsigned char *buffers_;
00268 unsigned int buffer_size_;
00269
00270 bool halt_motors_;
00271 unsigned int reset_state_;
00272
00273 unsigned timeout_;
00274 unsigned max_pd_retries_;
00275
00276 void publishDiagnostics();
00277 static void updateAccMax(double &max, const accumulator_set<double, stats<tag::max, tag::mean> > &acc);
00278 EthercatHardwareDiagnostics diagnostics_;
00279 EthercatHardwareDiagnosticsPublisher diagnostics_publisher_;
00280 ros::Time last_published_;
00281 ros::Time last_reset_;
00282
00283 realtime_tools::RealtimePublisher<std_msgs::Bool> motor_publisher_;
00284 ros::Time motor_last_published_;
00285
00286 EthercatOobCom *oob_com_;
00287
00288 pluginlib::ClassLoader<EthercatDevice> device_loader_;
00289 };
00290
00291 #endif