ethercat_hardware.h
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2008, Willow Garage, Inc.
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of the Willow Garage nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
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 #include <boost/regex.hpp>
00064 
00065 using namespace boost::accumulators;
00066  
00067 struct EthercatHardwareDiagnostics 
00068 {
00069   EthercatHardwareDiagnostics();
00070   void resetMaxTiming();
00071   accumulator_set<double, stats<tag::max, tag::mean> > pack_command_acc_; 
00072   accumulator_set<double, stats<tag::max, tag::mean> > txandrx_acc_;      
00073   accumulator_set<double, stats<tag::max, tag::mean> > unpack_state_acc_; 
00074   accumulator_set<double, stats<tag::max, tag::mean> > publish_acc_;      
00075   double max_pack_command_;
00076   double max_txandrx_;
00077   double max_unpack_state_;
00078   double max_publish_;
00079   int txandrx_errors_;
00080   unsigned device_count_;
00081   bool pd_error_;
00082   bool halt_after_reset_; 
00083   unsigned reset_motors_service_count_; 
00084   unsigned halt_motors_service_count_;  
00085   unsigned halt_motors_error_count_;    
00086   struct netif_counters counters_;
00087   bool input_thread_is_stopped_;
00088   bool motors_halted_; 
00089   const char* motors_halted_reason_; 
00090 
00091   static const bool collect_extra_timing_ = true;
00092 };
00093 
00094 
00108 class EthercatHardwareDiagnosticsPublisher
00109 {
00110 public:
00111 
00112   EthercatHardwareDiagnosticsPublisher(ros::NodeHandle &node);
00113   ~EthercatHardwareDiagnosticsPublisher();
00114 
00120   void initialize(const string &interface, unsigned int buffer_size, 
00121                   const std::vector<EthercatDevice*> &slaves, 
00122                   unsigned int num_ethercat_devices_,
00123                   unsigned timeout, unsigned max_pd_retries);
00124 
00132   void publish(const unsigned char *buffer, const EthercatHardwareDiagnostics &diagnostics);
00133  
00137   void stop();
00138    
00139 private:
00140 
00148   void publishDiagnostics();
00149 
00155   void diagnosticsThreadFunc();
00156 
00157 
00161   static void timingInformation(
00162         diagnostic_updater::DiagnosticStatusWrapper &status, 
00163         const string &key, 
00164         const accumulator_set<double, stats<tag::max, tag::mean> > &acc,
00165         double max);
00166 
00167   ros::NodeHandle node_;
00168 
00169   boost::mutex diagnostics_mutex_; 
00170   boost::condition_variable diagnostics_cond_;
00171   bool diagnostics_ready_;
00172   boost::thread diagnostics_thread_;
00173 
00174   ros::Publisher publisher_;
00175 
00176   EthercatHardwareDiagnostics diagnostics_; 
00177   unsigned char *diagnostics_buffer_;
00178   unsigned int buffer_size_;
00179   std::vector<EthercatDevice*> slaves_;
00180   unsigned int num_ethercat_devices_;
00181   string interface_;
00182 
00184   unsigned timeout_;
00186   unsigned max_pd_retries_;
00187 
00189   uint64_t last_dropped_packet_count_;
00191   ros::Time last_dropped_packet_time_; 
00193   static const unsigned dropped_packet_warning_hold_time_ = 10;  //keep warning up for 10 seconds
00194 
00195   diagnostic_msgs::DiagnosticArray diagnostic_array_;
00197   EthernetInterfaceInfo ethernet_interface_info_;
00198   vector<diagnostic_msgs::KeyValue> values_;
00199   diagnostic_updater::DiagnosticStatusWrapper status_;
00200 };
00201 
00202 
00203 class EthercatHardware
00204 {
00205 public:
00209   EthercatHardware(const std::string& name);
00210 
00214   ~EthercatHardware();
00215 
00221   void update(bool reset, bool halt);
00222 
00228   void init(char *interface, bool allow_unprogrammed);
00229 
00233   void collectDiagnostics();
00234 
00235   void printCounters(std::ostream &os=std::cout); 
00236 
00240   bool txandrx_PD(unsigned buffer_size, unsigned char* buffer, unsigned tries);
00241 
00251   bool publishTrace(int position, const string &reason, unsigned level, unsigned delay);
00252 
00253   pr2_hardware_interface::HardwareInterface *hw_;
00254 
00255 private:
00256   static void changeState(EtherCAT_SlaveHandler *sh, EC_State new_state);
00257 
00258   void loadNonEthercatDevices();
00259   EthercatDevice *configNonEthercatDevice(const std::string &product_id, const std::string &data);
00260 
00261   void haltMotors(bool error, const char* reason);
00262 
00263   ros::NodeHandle node_;
00264 
00265   struct netif *ni_;
00266   string interface_;
00267 
00268   EtherCAT_AL *al_;
00269   EtherCAT_Master *em_;
00270 
00271   EthercatDevice *configSlave(EtherCAT_SlaveHandler *sh);
00272   std::vector<EthercatDevice*> slaves_;
00273   unsigned int num_ethercat_devices_;
00274 
00275   unsigned char *this_buffer_;
00276   unsigned char *prev_buffer_;
00277   unsigned char *buffers_;
00278   unsigned int buffer_size_;
00279 
00280   bool halt_motors_;
00281   unsigned int reset_state_;
00282 
00283   unsigned timeout_;        
00284   unsigned max_pd_retries_; 
00285 
00286   void publishDiagnostics();  
00287   static void updateAccMax(double &max, const accumulator_set<double, stats<tag::max, tag::mean> > &acc);
00288   EthercatHardwareDiagnostics diagnostics_;
00289   EthercatHardwareDiagnosticsPublisher diagnostics_publisher_;
00290   ros::Time last_published_;
00291   ros::Time last_reset_;
00292 
00293   realtime_tools::RealtimePublisher<std_msgs::Bool> motor_publisher_;
00294 
00295   EthercatOobCom *oob_com_;  
00296 
00297   pluginlib::ClassLoader<EthercatDevice> device_loader_;
00298 };
00299 
00300 #endif /* ETHERCAT_HARDWARE_H */


ethercat_hardware
Author(s): Rob Wheeler (email: wheeler@willowgarage.com), Maintained by Derek King (email: dking@willowgarage.com)
autogenerated on Thu Jan 2 2014 11:39:31