ethercat_hardware.cpp
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 #include "ethercat_hardware/ethercat_hardware.h"
00036 
00037 #include <ethercat/ethercat_xenomai_drv.h>
00038 #include <dll/ethercat_dll.h>
00039 #include <dll/ethercat_device_addressed_telegram.h>
00040 
00041 #include <sstream>
00042 
00043 #include <net/if.h>
00044 #include <sys/ioctl.h>
00045 #include <boost/foreach.hpp>
00046 #include <boost/regex.hpp>
00047 
00048 EthercatHardwareDiagnostics::EthercatHardwareDiagnostics() :
00049 
00050   txandrx_errors_(0),
00051   device_count_(0),
00052   pd_error_(false),
00053   halt_after_reset_(false),
00054   reset_motors_service_count_(0), 
00055   halt_motors_service_count_(0),
00056   halt_motors_error_count_(0),
00057   motors_halted_(false),
00058   motors_halted_reason_("")
00059 {
00060   resetMaxTiming();
00061 }
00062 
00063 void EthercatHardwareDiagnostics::resetMaxTiming()
00064 {
00065   max_pack_command_  = 0.0;
00066   max_txandrx_       = 0.0;
00067   max_unpack_state_  = 0.0;
00068   max_publish_       = 0.0;
00069 }
00070 
00071 EthercatHardware::EthercatHardware(const std::string& name) :
00072   hw_(0), node_(ros::NodeHandle(name)),
00073   ni_(0), this_buffer_(0), prev_buffer_(0), buffer_size_(0), halt_motors_(true), reset_state_(0), 
00074   max_pd_retries_(10),
00075   diagnostics_publisher_(node_), 
00076   motor_publisher_(node_, "motors_halted", 1, true), 
00077   device_loader_("ethercat_hardware", "EthercatDevice")
00078 {
00079   
00080 }
00081 
00082 EthercatHardware::~EthercatHardware()
00083 {
00084   diagnostics_publisher_.stop();
00085   for (uint32_t i = 0; i < slaves_.size(); ++i)
00086   {
00087     EC_FixedStationAddress fsa(i + 1);
00088     EtherCAT_SlaveHandler *sh = em_->get_slave_handler(fsa);
00089     if (sh) sh->to_state(EC_PREOP_STATE);
00090     delete slaves_[i];
00091   }
00092   if (ni_)
00093   {
00094     close_socket(ni_);
00095   }
00096   delete[] buffers_;
00097   delete hw_;
00098   delete oob_com_;
00099   motor_publisher_.stop();
00100 }
00101 
00102 
00103 void EthercatHardware::changeState(EtherCAT_SlaveHandler *sh, EC_State new_state)
00104 {
00105   unsigned product_code = sh->get_product_code();
00106   unsigned serial = sh->get_serial();
00107   uint32_t revision = sh->get_revision();
00108   unsigned slave = sh->get_station_address()-1;
00109 
00110   if (!sh->to_state(new_state))
00111   {
00112     ROS_FATAL("Cannot goto state %d for slave #%d, product code: %u (0x%X), serial: %u (0x%X), revision: %d (0x%X)",
00113               new_state, slave, product_code, product_code, serial, serial, revision, revision);
00114     if ((product_code==0xbaddbadd) || (serial==0xbaddbadd) || (revision==0xbaddbadd))
00115       ROS_FATAL("Note: 0xBADDBADD indicates that the value was not read correctly from device.");
00116     exit(EXIT_FAILURE);
00117   }
00118 }
00119 
00120 void EthercatHardware::init(char *interface, bool allow_unprogrammed)
00121 {
00122   // open temporary socket to use with ioctl
00123   int sock = socket(PF_INET, SOCK_DGRAM, 0);
00124   if (sock < 0) {
00125     int error = errno;
00126     ROS_FATAL("Couldn't open temp socket : %s", strerror(error));
00127     sleep(1);
00128     exit(EXIT_FAILURE);    
00129   }
00130   
00131   struct ifreq ifr;
00132   strncpy(ifr.ifr_name, interface, IFNAMSIZ);
00133   if (ioctl(sock, SIOCGIFFLAGS, &ifr) < 0) {
00134     int error = errno;
00135     ROS_FATAL("Cannot get interface flags for %s: %s", interface, strerror(error));
00136     sleep(1);
00137     exit(EXIT_FAILURE);
00138   }
00139 
00140   close(sock);
00141   sock = -1;
00142 
00143   if (!(ifr.ifr_flags & IFF_UP)) {
00144     ROS_FATAL("Interface %s is not UP. Try : ifup %s", interface, interface);
00145     sleep(1);
00146     exit(EXIT_FAILURE);
00147   }
00148   if (!(ifr.ifr_flags & IFF_RUNNING)) {
00149     ROS_FATAL("Interface %s is not RUNNING. Is cable plugged in and device powered?", interface);
00150     sleep(1);
00151     exit(EXIT_FAILURE);
00152   }
00153 
00154 
00155   // Initialize network interface
00156   interface_ = interface;
00157   if ((ni_ = init_ec(interface)) == NULL)
00158   {
00159     ROS_FATAL("Unable to initialize interface: %s", interface);
00160     sleep(1);
00161     exit(EXIT_FAILURE);
00162   }
00163 
00164   oob_com_ = new EthercatOobCom(ni_);
00165 
00166   // Initialize Application Layer (AL)
00167   EtherCAT_DataLinkLayer::instance()->attach(ni_);
00168   if ((al_ = EtherCAT_AL::instance()) == NULL)
00169   {
00170     ROS_FATAL("Unable to initialize Application Layer (AL): %p", al_);
00171     sleep(1);
00172     exit(EXIT_FAILURE);
00173   }
00174 
00175   int num_ethercat_devices_ = al_->get_num_slaves();
00176   if (num_ethercat_devices_ == 0)
00177   {
00178     ROS_FATAL("Unable to locate any slaves");
00179     sleep(1);
00180     exit(EXIT_FAILURE);
00181   }
00182 
00183   // Initialize Master
00184   if ((em_ = EtherCAT_Master::instance()) == NULL)
00185   {
00186     ROS_FATAL("Unable to initialize EtherCAT_Master: %p", em_);
00187     sleep(1);
00188     exit(EXIT_FAILURE);
00189   }
00190 
00191   // Size slaves vector to hold appropriate number of device pointers
00192   slaves_.resize(num_ethercat_devices_, NULL);
00193 
00194   // Make temporary list of slave handles
00195   std::vector<EtherCAT_SlaveHandler*> slave_handles;
00196   for (unsigned int slave = 0; slave < slaves_.size(); ++slave)
00197   {
00198     EC_FixedStationAddress fsa(slave + 1);
00199     EtherCAT_SlaveHandler *sh = em_->get_slave_handler(fsa);
00200     if (sh == NULL)
00201     {
00202       ROS_FATAL("Unable to get slave handler #%d", slave);
00203       sleep(1);
00204       exit(EXIT_FAILURE);
00205     }
00206     slave_handles.push_back(sh);
00207   }
00208 
00209   // Configure EtherCAT slaves
00210   BOOST_FOREACH(EtherCAT_SlaveHandler *sh, slave_handles)
00211   {    
00212     unsigned slave = sh->get_station_address()-1;
00213     if ((slaves_[slave] = configSlave(sh)) == NULL)
00214     {      
00215       ROS_FATAL("Unable to configure slave #%d", slave);
00216       sleep(1);
00217       exit(EXIT_FAILURE);
00218     }
00219     buffer_size_ += slaves_[slave]->command_size_ + slaves_[slave]->status_size_;
00220   }
00221 
00222   // Configure any non-ethercat slaves (appends devices to slaves_ vector)
00223   loadNonEthercatDevices();
00224 
00225   // Move slave from INIT to PREOP
00226   BOOST_FOREACH(EtherCAT_SlaveHandler *sh, slave_handles)
00227   {
00228     changeState(sh,EC_PREOP_STATE);
00229   }
00230 
00231   // Move slave from PREOP to SAFEOP
00232   BOOST_FOREACH(EtherCAT_SlaveHandler *sh, slave_handles)
00233   {
00234     changeState(sh,EC_SAFEOP_STATE);
00235   }
00236 
00237   // Move slave from SAFEOP to OP
00238   // TODO : move to OP after initializing slave process data
00239   BOOST_FOREACH(EtherCAT_SlaveHandler *sh, slave_handles)
00240   {
00241     changeState(sh,EC_OP_STATE);
00242   }
00243 
00244   // Allocate buffers to send and receive commands
00245   buffers_ = new unsigned char[2 * buffer_size_];
00246   this_buffer_ = buffers_;
00247   prev_buffer_ = buffers_ + buffer_size_;
00248 
00249   // Make sure motors are disabled, also collect status data
00250   memset(this_buffer_, 0, 2 * buffer_size_);
00251   if (!txandrx_PD(buffer_size_, this_buffer_, 20))
00252   {
00253     ROS_FATAL("No communication with devices");
00254     sleep(1);
00255     exit(EXIT_FAILURE);
00256   }
00257   
00258   // prev_buffer should contain valid status data when update function is first used
00259   memcpy(prev_buffer_, this_buffer_, buffer_size_);
00260 
00261   // Create pr2_hardware_interface::HardwareInterface
00262   hw_ = new pr2_hardware_interface::HardwareInterface();
00263   hw_->current_time_ = ros::Time::now();
00264   last_published_ = hw_->current_time_;
00265 
00266   // Initialize slaves
00267   //set<string> actuator_names;
00268   for (unsigned int slave = 0; slave < slaves_.size(); ++slave)
00269   {
00270     if (slaves_[slave]->initialize(hw_, allow_unprogrammed) < 0)
00271     {
00272       EtherCAT_SlaveHandler *sh = slaves_[slave]->sh_;
00273       if (sh != NULL)
00274       {
00275         ROS_FATAL("Unable to initialize slave #%d, product code: %d, revision: %d, serial: %d",
00276                   slave, sh->get_product_code(), sh->get_revision(), sh->get_serial());
00277         sleep(1);
00278       } 
00279       else 
00280       {
00281         ROS_FATAL("Unable to initialize slave #%d", slave);
00282       }
00283       exit(EXIT_FAILURE);
00284     }
00285   }
00286 
00287 
00288   { // Initialization is now complete. Reduce timeout of EtherCAT txandrx for better realtime performance
00289     // Allow timeout to be configured at program load time with rosparam.  
00290     // This will allow tweaks for systems with different realtime performace
00291     static const int MAX_TIMEOUT = 100000;   // 100ms = 100,000us
00292     static const int DEFAULT_TIMEOUT = 20000; // default to timeout to 20000us = 20ms
00293     int timeout;
00294     if (!node_.getParam("realtime_socket_timeout", timeout))
00295     {
00296       timeout = DEFAULT_TIMEOUT; 
00297     }
00298     if ((timeout <= 1) || (timeout > MAX_TIMEOUT))
00299     {
00300       int old_timeout = timeout; 
00301       timeout = std::max(1, std::min(MAX_TIMEOUT, timeout));
00302       ROS_WARN("Invalid timeout (%d) for socket, using %d", old_timeout, timeout);
00303     }
00304     if (set_socket_timeout(ni_, timeout))
00305     {
00306       ROS_FATAL("Error setting socket timeout to %d", timeout);      
00307       sleep(1);
00308       exit(EXIT_FAILURE);
00309     }
00310     timeout_ = timeout;
00311 
00312     // When packet constaining process data is does not return after a given timeout, it is 
00313     // assumed to be dropped and the process data will automatically get re-sent.
00314     // After a number of retries, the driver will halt motors as a safety precaution.
00315     // 
00316     // The following code allows the number of process data retries to be changed with a rosparam. 
00317     // This is needed because lowering the txandrx timeout makes it more likely that a 
00318     // performance hickup in network or OS causes will cause the motors to halt.
00319     //
00320     // If number of retries is not specified, use a formula that allows 100ms of dropped packets
00321     int max_pd_retries = MAX_TIMEOUT / timeout;  // timeout is in nanoseconds : 20msec = 20000usec 
00322     static const int MAX_RETRIES=50, MIN_RETRIES=1;
00323     node_.getParam("max_pd_retries", max_pd_retries);
00324     // Make sure motor halt due to dropped packet takes less than 1/10 of a second
00325     if ((max_pd_retries * timeout) > (MAX_TIMEOUT))
00326     {
00327       max_pd_retries = MAX_TIMEOUT / timeout;
00328       ROS_WARN("Max PD retries is too large for given timeout.  Limiting value to %d", max_pd_retries);
00329     }
00330     if ((max_pd_retries < MIN_RETRIES) || (max_pd_retries > MAX_RETRIES))
00331     {
00332       max_pd_retries = std::max(MIN_RETRIES,std::min(MAX_RETRIES,max_pd_retries));
00333       ROS_WARN("Limiting max PD retries to %d", max_pd_retries);
00334     }
00335     max_pd_retries = std::max(MIN_RETRIES,std::min(MAX_RETRIES,max_pd_retries));
00336     max_pd_retries_ = max_pd_retries;
00337   }
00338 
00339   diagnostics_publisher_.initialize(interface_, buffer_size_, slaves_, num_ethercat_devices_, timeout_, max_pd_retries_);
00340 }
00341 
00342 
00343 EthercatHardwareDiagnosticsPublisher::EthercatHardwareDiagnosticsPublisher(ros::NodeHandle &node) :
00344   node_(node),
00345   diagnostics_ready_(false),
00346   publisher_(node_.advertise<diagnostic_msgs::DiagnosticArray>("/diagnostics", 1)),
00347   diagnostics_buffer_(NULL),
00348   last_dropped_packet_count_(0),
00349   last_dropped_packet_time_(0)
00350 {
00351 }
00352 
00353 EthercatHardwareDiagnosticsPublisher::~EthercatHardwareDiagnosticsPublisher()
00354 {
00355   delete[] diagnostics_buffer_;
00356 }
00357 
00358 void EthercatHardwareDiagnosticsPublisher::initialize(const string &interface, unsigned int buffer_size, 
00359                                                       const std::vector<EthercatDevice*> &slaves, 
00360                                                       unsigned int num_ethercat_devices, 
00361                                                       unsigned timeout, unsigned max_pd_retries)
00362 {
00363   interface_ = interface;
00364   buffer_size_ = buffer_size;
00365   slaves_ = slaves;
00366   num_ethercat_devices_ = num_ethercat_devices;
00367   timeout_ = timeout;
00368   max_pd_retries_ = max_pd_retries;
00369 
00370   diagnostics_buffer_ = new unsigned char[buffer_size_];
00371 
00372   // Initialize diagnostic data structures
00373   diagnostic_array_.status.reserve(slaves_.size() + 1);
00374   values_.reserve(10);
00375 
00376   ethernet_interface_info_.initialize(interface);
00377 
00378   diagnostics_thread_ = boost::thread(boost::bind(&EthercatHardwareDiagnosticsPublisher::diagnosticsThreadFunc, this));
00379 }
00380 
00381 void EthercatHardwareDiagnosticsPublisher::publish(
00382          const unsigned char *buffer, 
00383          const EthercatHardwareDiagnostics &diagnostics)
00384 {
00385   boost::try_to_lock_t try_lock;
00386   boost::unique_lock<boost::mutex> lock(diagnostics_mutex_, try_lock);
00387   if (lock.owns_lock())
00388   {
00389     // Make copies of diagnostic data for dianostic thread
00390     memcpy(diagnostics_buffer_, buffer, buffer_size_);
00391     diagnostics_ = diagnostics;
00392     // Trigger diagnostics publish thread
00393     diagnostics_ready_ = true;
00394     diagnostics_cond_.notify_one();
00395   }
00396 }
00397 
00398 void EthercatHardwareDiagnosticsPublisher::stop()
00399 {
00400   diagnostics_thread_.interrupt();
00401   diagnostics_thread_.join();
00402   publisher_.shutdown();
00403 }
00404 
00405 void EthercatHardwareDiagnosticsPublisher::diagnosticsThreadFunc()
00406 {
00407   try {
00408     while (1) {
00409       boost::unique_lock<boost::mutex> lock(diagnostics_mutex_);
00410       while (!diagnostics_ready_) {
00411         diagnostics_cond_.wait(lock);
00412       }
00413       diagnostics_ready_ = false;
00414       publishDiagnostics();
00415     }
00416   } catch (boost::thread_interrupted const&) {
00417     return;
00418   }
00419 }
00420 
00421 void EthercatHardwareDiagnosticsPublisher::timingInformation(
00422         diagnostic_updater::DiagnosticStatusWrapper &status, 
00423         const string &key, 
00424         const accumulator_set<double, stats<tag::max, tag::mean> > &acc,
00425         double max)
00426 {
00427   status.addf(key + " Avg (us)",       "%5.4f", extract_result<tag::mean>(acc) * 1e6); // Average over last 1 second
00428   status.addf(key + " 1 Sec Max (us)", "%5.4f", extract_result<tag::max>(acc) * 1e6);  // Max over last 1 second
00429   status.addf(key + " Max (us)",       "%5.4f", max * 1e6);                            // Max since start
00430 }
00431 
00432 void EthercatHardwareDiagnosticsPublisher::publishDiagnostics()
00433 {  
00434   ros::Time now(ros::Time::now());
00435 
00436   // Publish status of EtherCAT master
00437   status_.clearSummary();
00438   status_.clear();    
00439 
00440   status_.name = "EtherCAT Master";
00441   if (diagnostics_.motors_halted_)
00442   {
00443     std::ostringstream desc;
00444     desc << "Motors halted";
00445     if (diagnostics_.halt_after_reset_)
00446     {
00447       desc << " soon after reset";
00448     }
00449     desc << " (" << diagnostics_.motors_halted_reason_ << ")";
00450     status_.summary(status_.ERROR, desc.str());      
00451   } else {
00452     status_.summary(status_.OK, "OK");
00453   }
00454 
00455   if (diagnostics_.pd_error_)
00456   {
00457     status_.mergeSummary(status_.ERROR, "Error sending proccess data");
00458   }
00459 
00460   status_.add("Motors halted", diagnostics_.motors_halted_ ? "true" : "false");
00461   status_.addf("EtherCAT devices (expected)", "%d", num_ethercat_devices_); 
00462   status_.addf("EtherCAT devices (current)",  "%d", diagnostics_.device_count_); 
00463   ethernet_interface_info_.publishDiagnostics(status_);
00464   //status_.addf("Reset state", "%d", reset_state_);
00465 
00466   status_.addf("Timeout (us)", "%d", timeout_);
00467   status_.addf("Max PD Retries", "%d", max_pd_retries_);
00468 
00469   // Produce warning if number of devices changed after device initalization
00470   if (num_ethercat_devices_ != diagnostics_.device_count_) {
00471     status_.mergeSummary(status_.WARN, "Number of EtherCAT devices changed");
00472   }
00473 
00474   timingInformation(status_, "Roundtrip time", diagnostics_.txandrx_acc_, diagnostics_.max_txandrx_);
00475   if (diagnostics_.collect_extra_timing_)
00476   {
00477     timingInformation(status_, "Pack command time", diagnostics_.pack_command_acc_, diagnostics_.max_pack_command_);
00478     timingInformation(status_, "Unpack state time", diagnostics_.unpack_state_acc_, diagnostics_.max_unpack_state_);
00479     timingInformation(status_, "Publish time", diagnostics_.publish_acc_, diagnostics_.max_publish_);
00480   }
00481 
00482   status_.addf("EtherCAT Process Data txandrx errors", "%d", diagnostics_.txandrx_errors_);
00483 
00484   status_.addf("Reset motors service count", "%d", diagnostics_.reset_motors_service_count_);
00485   status_.addf("Halt motors service count", "%d", diagnostics_.halt_motors_service_count_);
00486   status_.addf("Halt motors error count", "%d", diagnostics_.halt_motors_error_count_);
00487 
00488   { // Publish ethercat network interface counters 
00489     const struct netif_counters *c = &diagnostics_.counters_;
00490     status_.add("Input Thread",       (diagnostics_.input_thread_is_stopped_ ? "Stopped" : "Running"));
00491     status_.addf("Sent Packets",        "%llu", (unsigned long long)c->sent);
00492     status_.addf("Received Packets",    "%llu", (unsigned long long)c->received);
00493     status_.addf("Collected Packets",   "%llu", (unsigned long long)c->collected);
00494     status_.addf("Dropped Packets",     "%llu", (unsigned long long)c->dropped);
00495     status_.addf("TX Errors",           "%llu", (unsigned long long)c->tx_error);
00496     status_.addf("TX Network Down",     "%llu", (unsigned long long)c->tx_net_down);
00497     status_.addf("TX Would Block",      "%llu", (unsigned long long)c->tx_would_block);
00498     status_.addf("TX No Buffers",       "%llu", (unsigned long long)c->tx_no_bufs);
00499     status_.addf("TX Queue Full",       "%llu", (unsigned long long)c->tx_full);
00500     status_.addf("RX Runt Packet",      "%llu", (unsigned long long)c->rx_runt_pkt);
00501     status_.addf("RX Not EtherCAT",     "%llu", (unsigned long long)c->rx_not_ecat);
00502     status_.addf("RX Other EML",        "%llu", (unsigned long long)c->rx_other_eml);
00503     status_.addf("RX Bad Index",        "%llu", (unsigned long long)c->rx_bad_index);
00504     status_.addf("RX Bad Sequence",     "%llu", (unsigned long long)c->rx_bad_seqnum);
00505     status_.addf("RX Duplicate Sequence", "%llu", (unsigned long long)c->rx_dup_seqnum);    
00506     status_.addf("RX Duplicate Packet", "%llu", (unsigned long long)c->rx_dup_pkt);    
00507     status_.addf("RX Bad Order",        "%llu", (unsigned long long)c->rx_bad_order);    
00508     status_.addf("RX Late Packet",      "%llu", (unsigned long long)c->rx_late_pkt);
00509     status_.addf("RX Late Packet RTT",  "%llu", (unsigned long long)c->rx_late_pkt_rtt_us);
00510     
00511     double rx_late_pkt_rtt_us_avg = 0.0;
00512     if (c->rx_late_pkt > 0) {
00513       rx_late_pkt_rtt_us_avg = ((double)c->rx_late_pkt_rtt_us_sum)/((double)c->rx_late_pkt);
00514     }
00515     status_.addf("RX Late Packet Avg RTT", "%f", rx_late_pkt_rtt_us_avg);
00516 
00517     // Check for newly dropped packets
00518     if (c->dropped > last_dropped_packet_count_)
00519     {
00520       last_dropped_packet_time_ = now;
00521       last_dropped_packet_count_ = c->dropped;
00522     }
00523   }
00524 
00525   // Create error message if packet has been dropped recently
00526   if ((last_dropped_packet_count_ > 0) && ((now - last_dropped_packet_time_).toSec() < dropped_packet_warning_hold_time_))
00527   {
00528     status_.mergeSummaryf(status_.WARN, "Dropped packets in last %d seconds", dropped_packet_warning_hold_time_);
00529   }
00530 
00531   diagnostic_array_.status.clear();
00532   diagnostic_array_.status.push_back(status_);
00533 
00534   // Also, collect diagnostic statuses of all EtherCAT device
00535   unsigned char *current = diagnostics_buffer_;
00536   for (unsigned int s = 0; s < slaves_.size(); ++s)
00537   {
00538     slaves_[s]->multiDiagnostics(diagnostic_array_.status, current);
00539     current += slaves_[s]->command_size_ + slaves_[s]->status_size_;
00540   }
00541 
00542   // Publish status of each EtherCAT device
00543   diagnostic_array_.header.stamp = ros::Time::now();
00544   publisher_.publish(diagnostic_array_);
00545 }
00546 
00547 void EthercatHardware::update(bool reset, bool halt)
00548 {
00549   // Update current time
00550   ros::Time update_start_time(ros::Time::now());
00551 
00552   unsigned char *this_buffer, *prev_buffer;
00553 
00554   // Convert HW Interface commands to MCB-specific buffers
00555   this_buffer = this_buffer_;
00556 
00557   if (halt)
00558   {
00559     ++diagnostics_.halt_motors_service_count_;
00560     haltMotors(false /*no error*/, "service request");
00561   }
00562 
00563   // Resetting devices should clear device errors and release devices from halt.
00564   // To reduce load on power system, release devices from halt, one at a time 
00565   const unsigned CYCLES_PER_HALT_RELEASE = 2; // Wait two cycles between releasing each device
00566   if (reset)
00567   {
00568     ++diagnostics_.reset_motors_service_count_;
00569     reset_state_ = CYCLES_PER_HALT_RELEASE * slaves_.size() + 5;
00570     last_reset_ = update_start_time;
00571     diagnostics_.halt_after_reset_ = false;
00572   }
00573   bool reset_devices = reset_state_ == CYCLES_PER_HALT_RELEASE * slaves_.size() + 3;
00574   if (reset_devices)
00575   {
00576     halt_motors_ = false;
00577     diagnostics_.motors_halted_ = false;
00578     diagnostics_.motors_halted_reason_ = "";
00579     diagnostics_.resetMaxTiming();
00580     diagnostics_.pd_error_ = false;
00581   }
00582 
00583   for (unsigned int s = 0; s < slaves_.size(); ++s)
00584   {
00585     // Pack the command structures into the EtherCAT buffer
00586     // Disable the motor if they are halted or coming out of reset
00587     bool halt_device = halt_motors_ || ((s*CYCLES_PER_HALT_RELEASE+1) < reset_state_);
00588     slaves_[s]->packCommand(this_buffer, halt_device, reset_devices);
00589     this_buffer += slaves_[s]->command_size_ + slaves_[s]->status_size_;
00590   }
00591 
00592   // Transmit process data
00593   ros::Time txandrx_start_time(ros::Time::now()); // Also end time for pack_command_stage
00594   diagnostics_.pack_command_acc_((txandrx_start_time-update_start_time).toSec());
00595 
00596   // Send/receive device proccess data
00597   bool success = txandrx_PD(buffer_size_, this_buffer_, max_pd_retries_);
00598 
00599   ros::Time txandrx_end_time(ros::Time::now());  // Also begining of unpack_state 
00600   diagnostics_.txandrx_acc_((txandrx_end_time - txandrx_start_time).toSec());
00601 
00602   hw_->current_time_ = txandrx_end_time;
00603 
00604   if (!success)
00605   {
00606     // If process data didn't get sent after multiple retries, stop motors
00607     haltMotors(true /*error*/, "communication error");
00608     diagnostics_.pd_error_ = true;
00609   }
00610   else
00611   {
00612     // Convert status back to HW Interface
00613     this_buffer = this_buffer_;
00614     prev_buffer = prev_buffer_;
00615     for (unsigned int s = 0; s < slaves_.size(); ++s)
00616     {
00617       if (!slaves_[s]->unpackState(this_buffer, prev_buffer) && !reset_devices)
00618       {
00619         haltMotors(true /*error*/, "device error");
00620       }
00621       this_buffer += slaves_[s]->command_size_ + slaves_[s]->status_size_;
00622       prev_buffer += slaves_[s]->command_size_ + slaves_[s]->status_size_;
00623     }
00624     
00625     if (reset_state_)
00626       --reset_state_;
00627     
00628     unsigned char *tmp = this_buffer_;
00629     this_buffer_ = prev_buffer_;
00630     prev_buffer_ = tmp;
00631   }
00632 
00633   ros::Time unpack_end_time;
00634   if (diagnostics_.collect_extra_timing_)
00635   {
00636     unpack_end_time = ros::Time::now();  // also start of publish time                            
00637     diagnostics_.unpack_state_acc_((unpack_end_time - txandrx_end_time).toSec());
00638   }
00639 
00640   if ((update_start_time - last_published_) > ros::Duration(1.0))
00641   {
00642     last_published_ = update_start_time;
00643     publishDiagnostics();
00644     motor_publisher_.lock();
00645     motor_publisher_.msg_.data = halt_motors_;
00646     motor_publisher_.unlockAndPublish();
00647   }
00648 
00649   if (diagnostics_.collect_extra_timing_)
00650   {
00651     ros::Time publish_end_time(ros::Time::now());  
00652     diagnostics_.publish_acc_((publish_end_time - unpack_end_time).toSec());
00653   }
00654 }
00655 
00656 
00657 void EthercatHardware::haltMotors(bool error, const char* reason)
00658 {
00659   if (!halt_motors_)
00660   {
00661     // wasn't already halted
00662     motor_publisher_.lock();
00663     motor_publisher_.msg_.data = halt_motors_;
00664     motor_publisher_.unlockAndPublish();
00665     
00666     diagnostics_.motors_halted_reason_=reason;
00667     if (error)
00668     {
00669       ++diagnostics_.halt_motors_error_count_;
00670       if ((ros::Time::now() - last_reset_) < ros::Duration(0.5))
00671       {
00672         // halted soon after reset
00673         diagnostics_.halt_after_reset_ = true;
00674       }
00675     }
00676   }
00677   
00678   diagnostics_.motors_halted_ = true;
00679   halt_motors_ = true;
00680 }
00681 
00682 
00683 void EthercatHardware::updateAccMax(double &max, const accumulator_set<double, stats<tag::max, tag::mean> > &acc)
00684 {
00685   max = std::max(max, extract_result<tag::max>(acc));
00686 }
00687 
00688 void EthercatHardware::publishDiagnostics()
00689 {
00690   // Update max timing values
00691   updateAccMax(diagnostics_.max_pack_command_, diagnostics_.pack_command_acc_);
00692   updateAccMax(diagnostics_.max_txandrx_,      diagnostics_.txandrx_acc_);
00693   updateAccMax(diagnostics_.max_unpack_state_, diagnostics_.unpack_state_acc_);
00694   updateAccMax(diagnostics_.max_publish_,      diagnostics_.publish_acc_);
00695 
00696   // Grab stats and counters from input thread
00697   diagnostics_.counters_ = ni_->counters;
00698   diagnostics_.input_thread_is_stopped_ = bool(ni_->is_stopped);
00699 
00700   diagnostics_.motors_halted_ = halt_motors_;
00701 
00702   // Pass diagnostic data to publisher thread
00703   diagnostics_publisher_.publish(this_buffer_, diagnostics_);
00704 
00705   // Clear statistics accumulators
00706   static accumulator_set<double, stats<tag::max, tag::mean> > blank;
00707   diagnostics_.pack_command_acc_ = blank;
00708   diagnostics_.txandrx_acc_      = blank;
00709   diagnostics_.unpack_state_acc_ = blank;
00710   diagnostics_.publish_acc_      = blank;
00711 }
00712 
00713 
00714 EthercatDevice *
00715 EthercatHardware::configSlave(EtherCAT_SlaveHandler *sh)
00716 {
00717   static int start_address = 0x00010000;
00718   EthercatDevice *p = NULL;
00719   unsigned product_code = sh->get_product_code();
00720   unsigned serial = sh->get_serial();
00721   uint32_t revision = sh->get_revision();
00722   unsigned slave = sh->get_station_address()-1;
00723 
00724   // The point of this code to find a class whose name matches the EtherCAT product ID 
00725   // for a given device.  
00726   // Thus device plugins would register themselves with PLUGIN_REGISTER_CLASS
00727   //
00728   //    PLUGINLIB_REGISTER_CLASS(class_name, class_type, base_class_type)
00729   // 
00730   // For the WG05 driver (productID = 6805005), this statement would look something like:
00731   //
00732   //   PLUGINLIB_DECLARE_CLASS(6805005, WG05, EthercatDevice);
00733   //
00734   // PLUGINLIB_DECLARE_CLASS macro is now depricated, PLUGINLIB_REGISTER_CLASS should be used instead : 
00735   // 
00736   //   PLUGINLIB_DECLARE_CLASS(package, class_name, class_type, base_class_type)
00737   //
00738   // Unfortunately, we don't know which ROS package that a particular driver is defined in.
00739   // To account for this, we search through the list of class names, one-by-one and find string where
00740   // last part of string matches product ID of device.  
00741   stringstream class_name_regex_str;
00742   class_name_regex_str << "(.*/)?" << product_code;
00743   boost::regex class_name_regex(class_name_regex_str.str(), boost::regex::extended);
00744 
00745   std::vector<std::string> classes = device_loader_.getDeclaredClasses();
00746   std::string matching_class_name;
00747 
00748   BOOST_FOREACH(const std::string &class_name, classes)
00749   {
00750     if (regex_match(class_name, class_name_regex))
00751     {
00752       if (matching_class_name.size() != 0)
00753       {
00754         ROS_ERROR("Found more than 1 EtherCAT driver for device with product code : %d", product_code);
00755         ROS_ERROR("First class name = '%s'.  Second class name = '%s'",
00756                   class_name.c_str(), matching_class_name.c_str());
00757       }
00758       matching_class_name = class_name;
00759     }
00760   }
00761 
00762   if (matching_class_name.size() != 0)
00763   {
00764     //ROS_WARN("Using driver class '%s' for device with product code %d", 
00765     //         matching_class_name.c_str(), product_code);
00766     try {      
00767       p = device_loader_.createClassInstance(matching_class_name);
00768     }
00769     catch (pluginlib::LibraryLoadException &e)
00770     {
00771       p = NULL;
00772       ROS_FATAL("Unable to load plugin for slave #%d, product code: %u (0x%X), serial: %u (0x%X), revision: %d (0x%X)",
00773                 slave, product_code, product_code, serial, serial, revision, revision);
00774       ROS_FATAL("%s", e.what());
00775     }    
00776   }
00777   else
00778   {
00779     if ((product_code==0xbaddbadd) || (serial==0xbaddbadd) || (revision==0xbaddbadd))
00780     {
00781       ROS_FATAL("Note: 0xBADDBADD indicates that the value was not read correctly from device.");
00782       ROS_FATAL("Perhaps you should power-cycle the MCBs");
00783     }
00784     else 
00785     {
00786       ROS_ERROR("Unable to load plugin for slave #%d, product code: %u (0x%X), serial: %u (0x%X), revision: %d (0x%X)",
00787                 slave, product_code, product_code, serial, serial, revision, revision);
00788       ROS_ERROR("Possible classes:");
00789       BOOST_FOREACH(const std::string &class_name, classes)
00790       {
00791         ROS_ERROR("  %s", class_name.c_str());
00792       }
00793       
00794       // TODO, use default plugin for ethercat devices that have no driver. 
00795       // This way, the EtherCAT chain still works. 
00796     }                
00797   }
00798 
00799   if (p != NULL)
00800   {
00801     p->construct(sh, start_address);
00802   }
00803 
00804   return p;
00805 }
00806 
00807 
00808 EthercatDevice *
00809 EthercatHardware::configNonEthercatDevice(const std::string &name, const std::string &type)
00810 {
00811   EthercatDevice *p = NULL;
00812   try {
00813     p = device_loader_.createClassInstance(type);
00814   }
00815   catch (pluginlib::LibraryLoadException &e)
00816   {
00817     p = NULL;
00818     ROS_FATAL("Unable to load plugin for non-EtherCAT device '%s' with type: %s : %s"
00819               , name.c_str(), type.c_str(), e.what());
00820   }
00821   if (p) {
00822     ROS_INFO("Creating non-EtherCAT device '%s' of type '%s'", name.c_str(), type.c_str());
00823     ros::NodeHandle nh(node_, "non_ethercat_devices/"+name);
00824     p->construct(nh);
00825   }
00826   return p;
00827 }
00828 
00829 
00830 // Do this to get access to Struct (std::map) element of XmlRpcValue
00831 class MyXmlRpcValue : public XmlRpc::XmlRpcValue
00832 {
00833 public:
00834   MyXmlRpcValue(XmlRpc::XmlRpcValue &value) : XmlRpc::XmlRpcValue(value) { }
00835   XmlRpcValue::ValueStruct &getMap() {return *_value.asStruct;}
00836 };
00837 
00838 void EthercatHardware::loadNonEthercatDevices()
00839 {
00840   // non-EtherCAT device drivers are descibed via struct named "non_ethercat_devices"
00841   // Each element of "non_ethercat_devices" must be a struct that contains a "type" field.
00842   // The element struct can also contain other elements that are used as configuration parameters
00843   // for the device plugin.
00844   // Example:
00845   //   non_ethercat_devices:
00846   //     netft1:
00847   //       type: NetFT
00848   //       address: 192.168.1.1
00849   //       publish_period: 0.01
00850   //     netft2:
00851   //       type: NetFT
00852   //       address: 192.168.1.2
00853   //       publish_period: 0.01
00854   //     dummy_device:
00855   //       type: Dummy
00856   //       msg: "This a dummy device"
00857   //
00858   //   The name of the device is used to create a ros::NodeHandle that is
00859   //    that is passed to construct function of device class.
00860   //   NOTE: construct function is not the same as C++ construtor
00861   if (!node_.hasParam("non_ethercat_devices"))
00862   {
00863     // It is perfectly fine if there is no list of non-ethercat devices
00864     return;
00865   }
00866 
00867   XmlRpc::XmlRpcValue devices;
00868   node_.getParam("non_ethercat_devices", devices);
00869   if (devices.getType() != XmlRpc::XmlRpcValue::TypeStruct)
00870   {
00871     ROS_ERROR("Rosparam 'non_ethercat_devices' is not a struct type");
00872     return;
00873   }
00874 
00875   MyXmlRpcValue my_devices(devices);
00876   typedef XmlRpc::XmlRpcValue::ValueStruct::value_type map_item_t ;
00877   BOOST_FOREACH(map_item_t &item, my_devices.getMap())
00878   {    
00879     const std::string &name(item.first);
00880     XmlRpc::XmlRpcValue &device_info(item.second);
00881 
00882     if (device_info.getType() != XmlRpc::XmlRpcValue::TypeStruct)
00883     {
00884       ROS_ERROR("non_ethercat_devices/%s is not a struct type", name.c_str());
00885       continue;
00886     }
00887 
00888     if (!device_info.hasMember("type"))
00889     {
00890       ROS_ERROR("non_ethercat_devices/%s 'type' element", name.c_str());
00891       continue;
00892     }
00893     
00894     std::string type(static_cast<std::string>(device_info["type"])); 
00895 
00896     EthercatDevice *new_device = configNonEthercatDevice(name,type);
00897     if (new_device != NULL)
00898     {
00899       slaves_.push_back(new_device);
00900     }
00901   }
00902 }
00903 
00904 
00905 void EthercatHardware::collectDiagnostics()
00906 {
00907   if (NULL == oob_com_)
00908     return;
00909 
00910   { // Count number of devices 
00911     EC_Logic *logic = EC_Logic::instance();
00912     unsigned char p[1];
00913     EC_UINT length = sizeof(p);
00914     
00915     // Build read telegram, use slave position
00916     APRD_Telegram status(logic->get_idx(), // Index
00917                          0, // Slave position on ethercat chain (auto increment address)
00918                          0, // ESC physical memory address (start address)
00919                          logic->get_wkc(), // Working counter
00920                          length, // Data Length,
00921                          p); // Buffer to put read result into
00922     
00923     // Put read telegram in ethercat/ethernet frame
00924     EC_Ethernet_Frame frame(&status);    
00925     oob_com_->txandrx(&frame);
00926 
00927     // Worry about locking for single value?
00928     diagnostics_.device_count_ = status.get_adp();
00929   }
00930 
00931   for (unsigned i = 0; i < slaves_.size(); ++i)
00932   {    
00933     EthercatDevice * d(slaves_[i]);
00934     d->collectDiagnostics(oob_com_);
00935   }
00936 }
00937 
00938 
00939 // Prints (error) counter infomation of network interface driver
00940 void EthercatHardware::printCounters(std::ostream &os) 
00941 {  
00942   const struct netif_counters &c(ni_->counters);      
00943   os << "netif counters :" << endl
00944      << " sent          = " << c.sent << endl
00945      << " received      = " << c.received << endl
00946      << " collected     = " << c.collected << endl
00947      << " dropped       = " << c.dropped << endl
00948      << " tx_error      = " << c.tx_error << endl
00949      << " tx_net_down   = " << c.tx_net_down << endl
00950      << " tx_would_block= " << c.tx_would_block << endl
00951      << " tx_no_bufs    = " << c.tx_no_bufs << endl
00952      << " tx_full       = " << c.tx_full << endl
00953      << " rx_runt_pkt   = " << c.rx_runt_pkt << endl
00954      << " rx_not_ecat   = " << c.rx_not_ecat << endl
00955      << " rx_other_eml  = " << c.rx_other_eml << endl
00956      << " rx_bad_index  = " << c.rx_bad_index << endl
00957      << " rx_bad_seqnum = " << c.rx_bad_seqnum << endl
00958      << " rx_dup_seqnum = " << c.rx_dup_seqnum << endl
00959      << " rx_dup_pkt    = " << c.rx_dup_pkt << endl
00960      << " rx_bad_order  = " << c.rx_bad_order << endl
00961      << " rx_late_pkt   = " << c.rx_late_pkt << endl;
00962 }
00963 
00964 
00965 bool EthercatHardware::txandrx_PD(unsigned buffer_size, unsigned char* buffer, unsigned tries)
00966 {
00967   // Try multiple times to get proccess data to device
00968   bool success = false;
00969   for (unsigned i=0; i<tries && !success; ++i) {
00970     // Try transmitting process data
00971     success = em_->txandrx_PD(buffer_size_, this_buffer_);
00972     if (!success) {
00973       ++diagnostics_.txandrx_errors_;
00974     } 
00975     // Transmit new OOB data
00976     oob_com_->tx();
00977   }
00978   return success;
00979 }
00980 
00981 
00982 bool EthercatHardware::publishTrace(int position, const string &reason, unsigned level, unsigned delay)
00983 {
00984   if (position >= (int)slaves_.size())
00985   {
00986     ROS_WARN("Invalid device position %d.  Use 0-%d, or -1.", position, int(slaves_.size())-1);
00987     return false;
00988   }
00989 
00990   if (level > 2) 
00991   {
00992     ROS_WARN("Invalid level : %d.  Using level=2 (ERROR).", level);
00993     level = 2;
00994   }
00995 
00996   string new_reason("Manually triggered : " + reason);
00997   
00998   bool retval = false;
00999   if (position < 0) 
01000   {
01001     for (unsigned i=0; i<slaves_.size(); ++i)
01002     {
01003       if (slaves_[i]->publishTrace(new_reason,level,delay))
01004       {
01005         retval = true;
01006       }
01007     }
01008   }
01009   else 
01010   {
01011     retval = slaves_[position]->publishTrace(new_reason,level,delay);
01012     if (!retval) 
01013     {
01014       ROS_WARN("Device %d does not support publishing trace", position);
01015     }
01016   }
01017   return retval;
01018 }


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