ethercat_manager.cpp
Go to the documentation of this file.
00001 #include "robotiq_ethercat/ethercat_manager.h"
00002 
00003 #include <string.h>
00004 
00005 #include <unistd.h>
00006 
00007 #include <boost/ref.hpp>
00008 #include <boost/interprocess/sync/scoped_lock.hpp>
00009 
00010 #include <soem/ethercattype.h>
00011 #include <soem/nicdrv.h>
00012 #include <soem/ethercatbase.h>
00013 #include <soem/ethercatmain.h>
00014 #include <soem/ethercatdc.h>
00015 #include <soem/ethercatcoe.h>
00016 #include <soem/ethercatfoe.h>
00017 #include <soem/ethercatconfig.h>
00018 #include <soem/ethercatprint.h>
00019 
00020 #include <ros/ros.h>
00021 
00022 namespace 
00023 {
00024   static const unsigned THREAD_SLEEP_TIME = 10000; // 10 ms
00025   static const unsigned EC_TIMEOUTMON = 500;
00026 
00027   void handleErrors()
00028   {
00029     /* one ore more slaves are not responding */
00030     ec_group[0].docheckstate = FALSE;
00031     ec_readstate();
00032     for (int slave = 1; slave <= ec_slavecount; slave++)
00033     {
00034       if ((ec_slave[slave].group == 0) && (ec_slave[slave].state != EC_STATE_OPERATIONAL))
00035       {
00036         ec_group[0].docheckstate = TRUE;
00037         if (ec_slave[slave].state == (EC_STATE_SAFE_OP + EC_STATE_ERROR))
00038         {
00039           ROS_ERROR("ERROR : slave %d is in SAFE_OP + ERROR, attempting ack.\n", slave);
00040           ec_slave[slave].state = (EC_STATE_SAFE_OP + EC_STATE_ACK);
00041           ec_writestate(slave);
00042         }
00043         else if(ec_slave[slave].state == EC_STATE_SAFE_OP)
00044         {
00045           ROS_WARN("WARNING : slave %d is in SAFE_OP, change to OPERATIONAL.\n", slave);
00046           ec_slave[slave].state = EC_STATE_OPERATIONAL;
00047           ec_writestate(slave);
00048         }
00049         else if(ec_slave[slave].state > 0)
00050         {
00051           if (ec_reconfig_slave(slave, EC_TIMEOUTMON))
00052           {
00053             ec_slave[slave].islost = FALSE;
00054             ROS_INFO("MESSAGE : slave %d reconfigured\n",slave);
00055           }
00056         }
00057         else if(!ec_slave[slave].islost)
00058         {
00059           /* re-check state */
00060           ec_statecheck(slave, EC_STATE_OPERATIONAL, EC_TIMEOUTRET);
00061           if (!ec_slave[slave].state)
00062           {
00063             ec_slave[slave].islost = TRUE;
00064             ROS_ERROR("ERROR : slave %d lost\n",slave);
00065           }
00066         }
00067       }
00068       if (ec_slave[slave].islost)
00069       {
00070         if(!ec_slave[slave].state)
00071         {
00072           if (ec_recover_slave(slave, EC_TIMEOUTMON))
00073           {
00074             ec_slave[slave].islost = FALSE;
00075             ROS_INFO("MESSAGE : slave %d recovered\n",slave);
00076           }
00077         }
00078         else
00079         {
00080           ec_slave[slave].islost = FALSE;
00081           ROS_INFO("MESSAGE : slave %d found\n",slave);
00082         }
00083       }
00084     }
00085   }
00086 
00087   void cycleWorker(boost::mutex& mutex, bool& stop_flag)
00088   {
00089     while (!stop_flag) 
00090     {
00091       int expected_wkc = (ec_group[0].outputsWKC * 2) + ec_group[0].inputsWKC;
00092       int sent, wkc;
00093       {
00094         boost::mutex::scoped_lock lock(mutex);
00095         sent = ec_send_processdata();
00096         wkc = ec_receive_processdata(EC_TIMEOUTRET);
00097       }
00098 
00099       if (wkc < expected_wkc)
00100       {
00101         handleErrors();
00102       }
00103 
00104       usleep(THREAD_SLEEP_TIME);
00105     }
00106   }
00107 
00108 } // end of anonymous namespace
00109 
00110 
00111 robotiq_ethercat::EtherCatManager::EtherCatManager(const std::string& ifname)
00112   : ifname_(ifname)
00113   , stop_flag_(false)
00114 {
00115   if (initSoem(ifname)) 
00116   {
00117     cycle_thread_ = boost::thread(cycleWorker, 
00118                                   boost::ref(iomap_mutex_),
00119                                   boost::ref(stop_flag_));
00120   } 
00121   else 
00122   {
00123     // construction failed
00124     throw EtherCatError("Could not initialize SOEM");
00125   }
00126 }
00127 
00128 robotiq_ethercat::EtherCatManager::~EtherCatManager()
00129 {
00130   stop_flag_ = true;
00131   ec_close();
00132   cycle_thread_.join();
00133 }
00134 
00135 void robotiq_ethercat::EtherCatManager::write(int slave_no, uint8_t channel, uint8_t value)
00136 {
00137   boost::mutex::scoped_lock lock(iomap_mutex_);
00138   ec_slave[slave_no].outputs[channel] = value;
00139 }
00140 
00141 uint8_t robotiq_ethercat::EtherCatManager::readInput(int slave_no, uint8_t channel) const
00142 {
00143   boost::mutex::scoped_lock lock(iomap_mutex_);
00144   return ec_slave[slave_no].inputs[channel];
00145 }
00146 
00147 uint8_t robotiq_ethercat::EtherCatManager::readOutput(int slave_no, uint8_t channel) const
00148 {
00149   boost::mutex::scoped_lock lock(iomap_mutex_);
00150   return ec_slave[slave_no].outputs[channel];
00151 }
00152 
00153 bool robotiq_ethercat::EtherCatManager::initSoem(const std::string& ifname)
00154 {
00155   // Copy string contents because SOEM library doesn't 
00156     // practice const correctness
00157     const static unsigned MAX_BUFF_SIZE = 1024;
00158     char buffer[MAX_BUFF_SIZE];
00159     size_t name_size = ifname_.size();
00160     if (name_size > sizeof(buffer) - 1) 
00161     {
00162       ROS_ERROR("Ifname %s exceeds maximum size of %u bytes", ifname.c_str(), MAX_BUFF_SIZE);
00163       return false;
00164     }
00165     std::strncpy(buffer, ifname_.c_str(), MAX_BUFF_SIZE);
00166 
00167     ROS_INFO("Initializing etherCAT master");
00168 
00169     if (!ec_init(buffer))
00170     {
00171       ROS_ERROR("Could not initialize ethercat driver");
00172       return false;
00173     }
00174 
00175     /* find and auto-config slaves */
00176     if (ec_config_init(FALSE) <= 0)
00177     {
00178       ROS_WARN("No slaves found on %s", ifname_.c_str());
00179       return false;
00180     }
00181 
00182     ROS_INFO("SOEM found and configured %d slaves", ec_slavecount);
00183 
00184     unsigned map_size = ec_slave[0].Obytes + ec_slave[0].Ibytes;
00185     iomap_.reset(new uint8_t[map_size]);
00186 
00187     int iomap_size = ec_config_map(iomap_.get());
00188     ROS_INFO("SOEM IOMap size: %d", iomap_size);
00189 
00190     // locates dc slaves - ???
00191     ec_configdc();
00192 
00193     // '0' here addresses all slaves
00194     if (ec_statecheck(0, EC_STATE_SAFE_OP, EC_TIMEOUTSTATE*4) != EC_STATE_SAFE_OP)
00195     {
00196       ROS_ERROR("Could not set EC_STATE_SAFE_OP");
00197       return false;
00198     }
00199 
00200     /* 
00201       This section attempts to bring all slaves to operational status. It does so
00202       by attempting to set the status of all slaves (ec_slave[0]) to operational,
00203       then proceeding through 40 send/recieve cycles each waiting up to 50 ms for a
00204       response about the status. 
00205     */
00206     ec_slave[0].state = EC_STATE_OPERATIONAL;
00207     ec_send_processdata();
00208     ec_receive_processdata(EC_TIMEOUTRET);
00209 
00210     ec_writestate(0);
00211     int chk = 40;
00212     do {
00213       ec_send_processdata();
00214       ec_receive_processdata(EC_TIMEOUTRET);
00215       ec_statecheck(0, EC_STATE_OPERATIONAL, 50000); // 50 ms wait for state check
00216     } while (chk-- && (ec_slave[0].state != EC_STATE_OPERATIONAL));
00217 
00218     if(ec_statecheck(0,EC_STATE_OPERATIONAL, EC_TIMEOUTSTATE) != EC_STATE_OPERATIONAL)
00219     {
00220       ROS_ERROR_STREAM("OPERATIONAL state not set, exiting");
00221       return false;
00222     }
00223 
00224     ROS_INFO("Finished configuration successfully");
00225     return true;
00226 }


robotiq_ethercat
Author(s): Jonathan Meyer
autogenerated on Thu Jun 6 2019 17:57:57