motorconf.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 <map>
00036 #include <stdio.h>
00037 #include <getopt.h>
00038 #include <sys/mman.h>
00039 
00040 #include <tinyxml.h>
00041 
00042 #include <ethercat/ethercat_xenomai_drv.h>
00043 #include <dll/ethercat_dll.h>
00044 #include <al/ethercat_AL.h>
00045 #include <al/ethercat_master.h>
00046 #include <al/ethercat_slave_handler.h>
00047 
00048 #include "ethercat_hardware/motor_heating_model.h"
00049 #include <ethercat_hardware/wg0x.h>
00050 #include <ethercat_hardware/wg05.h>
00051 #include <ethercat_hardware/wg06.h>
00052 #include <ethercat_hardware/wg021.h>
00053 #include <ethercat_hardware/wg014.h>
00054 
00055 #include <boost/crc.hpp>
00056 #include <boost/foreach.hpp>
00057 
00058 #include <net/if.h>
00059 #include <sys/ioctl.h>
00060 #include <netinet/in.h>
00061 
00062 #include <log4cxx/logger.h>
00063 
00064 using namespace ethercat_hardware;
00065 
00066 vector<EthercatDevice *> devices;
00067 
00068 struct Actuator {
00069   string motor;
00070   string board;
00071   bool enforce_heating_model;
00072 };
00073 typedef pair<string, Actuator> ActuatorPair;
00074 map<string, Actuator> actuators;
00075 
00076 struct Config 
00077 {
00078   Config(const WG0XActuatorInfo &actuator_info, const MotorHeatingModelParametersEepromConfig &heating_config) :
00079     actuator_info_(actuator_info), heating_config_(heating_config)
00080   {  }
00081   Config() {}
00082   WG0XActuatorInfo actuator_info_;
00083   MotorHeatingModelParametersEepromConfig heating_config_;
00084 };
00085 typedef pair<string, Config> MotorPair;
00086 map<string, Config> motors;
00087 
00088 void init(char *interface)
00089 {
00090   // open temporary socket to use with ioctl
00091   int sock = socket(PF_INET, SOCK_DGRAM, 0);
00092   if (sock < 0) {
00093     int error = errno;
00094     fprintf(stderr,"Couldn't open temp socket : %s", strerror(error));
00095     exit(-1);
00096   }
00097   
00098   struct ifreq ifr;
00099   strncpy(ifr.ifr_name, interface, IFNAMSIZ);
00100   if (ioctl(sock, SIOCGIFFLAGS, &ifr) < 0) {
00101     int error = errno;
00102     fprintf(stderr,"Cannot get interface flags for %s: %s\n", interface, strerror(error));
00103     exit(-1);
00104   }
00105 
00106   close(sock);
00107   sock = -1;
00108 
00109   if (!(ifr.ifr_flags & IFF_UP)) {
00110     fprintf(stderr,"Interface %s is not UP. Try : ifup %s\n", interface, interface);
00111     exit(-1);
00112   }
00113   if (!(ifr.ifr_flags & IFF_RUNNING)) {
00114     fprintf(stderr,"Interface %s is not RUNNING. Is cable plugged in and device powered?\n", interface);
00115     exit(-1);
00116   }
00117 
00118   struct netif *ni;
00119 
00120   // Initialize network interface
00121   if ((ni = init_ec(interface)) == NULL)
00122   {
00123     fprintf(stderr, "Unable to initialize interface: %s\n", interface);
00124     exit(-1);
00125   }
00126 
00127   // Initialize Application Layer (AL)
00128   EtherCAT_DataLinkLayer::instance()->attach(ni);
00129   EtherCAT_AL *al;
00130   if ((al = EtherCAT_AL::instance()) == NULL)
00131   {
00132     fprintf(stderr, "Unable to initialize Application Layer (AL): %p\n", al);
00133     exit(-1);
00134   }
00135 
00136   uint32_t num_slaves = al->get_num_slaves();
00137   if (num_slaves == 0)
00138   {
00139     fprintf(stderr, "Unable to locate any slaves\n");
00140     exit(-1);
00141   }
00142 
00143   // Initialize Master
00144   EtherCAT_Master *em;
00145   if ((em = EtherCAT_Master::instance()) == NULL)
00146   {
00147     fprintf(stderr, "Unable to initialize EtherCAT_Master: %p", em);
00148     exit(-1);
00149   }
00150 
00151   static int start_address = 0x00010000;
00152 
00153   for (unsigned int slave = 0; slave < num_slaves; ++slave)
00154   {
00155     EC_FixedStationAddress fsa(slave + 1);
00156     EtherCAT_SlaveHandler *sh = em->get_slave_handler(fsa);
00157     if (sh == NULL)
00158     {
00159       fprintf(stderr, "Unable to get slave handler #%d", slave);
00160       exit(-1);
00161     }
00162 
00163     if (sh->get_product_code() == WG05::PRODUCT_CODE)
00164     {
00165       WG05 *dev = new WG05();
00166       dev->construct(sh, start_address);
00167       devices.push_back(dev);
00168     }
00169     else if (sh->get_product_code() == WG06::PRODUCT_CODE)
00170     {
00171       WG06 *dev = new WG06();
00172       dev->construct(sh, start_address);
00173       devices.push_back(dev);
00174     }
00175     else if (sh->get_product_code() == WG021::PRODUCT_CODE)
00176     {
00177       WG021 *dev = new WG021();
00178       dev->construct(sh, start_address);
00179       devices.push_back(dev);
00180     }
00181     else if (sh->get_product_code() == WG014::PRODUCT_CODE)
00182     {
00183       WG014 *dev = new WG014();
00184       dev->construct(sh, start_address);
00185       devices.push_back(dev);
00186     }
00187     else
00188     {
00189       devices.push_back(NULL);
00190     }
00191   }
00192 
00193   BOOST_FOREACH(EthercatDevice *device, devices)
00194   {
00195     if (!device) continue;
00196     if (!device->sh_->to_state(EC_OP_STATE))
00197     {
00198       fprintf(stderr, "Unable set device %d into OP_STATE", device->sh_->get_ring_position());
00199     }
00200   }
00201   
00202   BOOST_FOREACH(EthercatDevice *device, devices)
00203   {
00204     if (!device) continue;
00205     device->use_ros_ = false;
00206     device->initialize(NULL, true);
00207   }
00208 }
00209 
00210 string boardName(EthercatDevice *d)
00211 {
00212   if (dynamic_cast<WG021 *>(d)) {
00213     return "wg021";
00214   } else if (dynamic_cast<WG06 *>(d)) {
00215     return "wg006";
00216   } else if (dynamic_cast<WG05 *>(d)) {
00217     return "wg005";
00218   }
00219   return "unknown";
00220 }
00221 
00222 
00223 WG0X* getWGDevice(int device)
00224 {
00225   uint32_t num_slaves = EtherCAT_AL::instance()->get_num_slaves();
00226   if ((device >= (int)num_slaves) || (device < 0)) 
00227   {
00228     ROS_FATAL("Invalid device number %d.  Must be value between 0 and %d", device, num_slaves-1);
00229     return NULL;
00230   }
00231  
00232   if (devices[device] == NULL)
00233   {
00234     ROS_FATAL("There is no device at position #%d", device);
00235     return NULL;
00236   }
00237   
00238   WG0X *wg = dynamic_cast<WG0X *>(devices[device]);
00239   if (wg==NULL) 
00240   {
00241     ROS_FATAL("The device a position #%d is not programmable", device);
00242     return NULL;
00243   }    
00244 
00245   return wg;
00246 }
00247 
00248 
00249 bool programDevice(int device, 
00250                    const Config &config, 
00251                    char *name, string expected_board, bool enforce_heating_model)
00252 {
00253   WG0X *wg = getWGDevice(device);
00254   if (wg == NULL)
00255   {
00256     return false;
00257   }
00258 
00259   string board = boardName(devices[device]);
00260   if (expected_board != board)
00261   {
00262     ROS_FATAL("Device #%02d is a %s, but %s expects a %s\n", device, board.c_str(), name, expected_board.c_str());
00263     return false;
00264   }
00265   ROS_INFO("Programming device %d, to be named: %s\n", device, name);
00266 
00267   WG0XActuatorInfo actuator_info = config.actuator_info_;
00268   if (strlen(name) >= sizeof(actuator_info.name_))
00269   {
00270     ROS_FATAL("Device name '%s' is too long", name);
00271   }
00272   strncpy(actuator_info.name_, name, sizeof(actuator_info.name_));
00273   actuator_info.generateCRC();
00274 
00275   ROS_INFO("Programming actuator version %d.%d", 
00276            int(actuator_info.major_), int(actuator_info.minor_));
00277 
00278   EthercatDirectCom com(EtherCAT_DataLinkLayer::instance());
00279   if (!wg->program(&com, actuator_info))
00280   {
00281     ROS_FATAL("Error writing actuator info to device #%d", device);
00282     return false;
00283   }
00284 
00285   MotorHeatingModelParametersEepromConfig heating_config = config.heating_config_;
00286   heating_config.enforce_ = enforce_heating_model;
00287   heating_config.generateCRC();
00288   if (!wg->program(&com, config.heating_config_))
00289   {
00290     ROS_FATAL("Writing heating model config to device #%d", device);
00291     return false;
00292   }
00293 
00294   return true;
00295 }
00296 
00297 
00298 // Uses ActuatorInfo name already stored in device
00299 // to find appropriate motor heating model config
00300 // Then only updates motor heating model config
00301 bool updateHeatingConfig(int device)
00302 {
00303   WG0XActuatorInfo actuator_info;
00304   EthercatDirectCom com(EtherCAT_DataLinkLayer::instance());
00305   WG0X *wg = getWGDevice(device);
00306   if (wg == NULL)
00307   {
00308     ROS_WARN("Skipping update of device %d", device);      
00309     return false;
00310   }
00311     
00312   if (!wg->readActuatorInfoFromEeprom(&com, actuator_info))
00313   {
00314     ROS_ERROR("Could not read actuator info from device %d", device);      
00315     return false;
00316   }
00317 
00318   if (!actuator_info.verifyCRC())
00319   {
00320     ROS_ERROR("Device %d has not actuator configuration", device);
00321     return false;
00322   }
00323 
00324   if (actuators.find(actuator_info.name_) == actuators.end())
00325   {
00326     ROS_ERROR("Could not find actuator info for device %d with name '%s'",
00327               device, actuator_info.name_);
00328     return false;
00329   }  
00330 
00331   const Actuator &actuator(actuators[actuator_info.name_]);
00332   const string &motor_name(actuator.motor);
00333   bool enforce_heating_model = actuator.enforce_heating_model;
00334 
00335   if (motors.find(motor_name) == motors.end())
00336   {
00337     ROS_ERROR("Could not find motor '%s' for device %d with actuator name '%s'",
00338               motor_name.c_str(), device, actuator_info.name_);
00339     return false;
00340   }
00341 
00342   const Config &config(motors[motor_name]);  
00343 
00344   if (strcmp(config.actuator_info_.motor_model_, actuator_info.motor_model_) != 0)
00345   {
00346     ROS_ERROR("For device %d '%s' : the motor name stored in EEPROM '%s' does not match the motor name '%s' from XML file", 
00347               device, actuator_info.name_, actuator_info.motor_model_, config.actuator_info_.motor_model_);
00348     return false;
00349   }
00350    
00351   MotorHeatingModelParametersEepromConfig heating_config = config.heating_config_;
00352   heating_config.enforce_ = enforce_heating_model;
00353   heating_config.generateCRC(); 
00354   if (!wg->program(&com, heating_config))
00355   {
00356     ROS_FATAL("Writing heating model config to device #%d", device);
00357     return false;
00358   }
00359 
00360   ROS_INFO("Updated device %d (%s) with heating config for motor '%s'", 
00361            device, actuator_info.name_, config.actuator_info_.motor_model_);
00362 
00363   return true;
00364 }
00365 
00366 
00367 // Updates heating configuration of all devices
00368 bool updateAllHeatingConfig()
00369 {
00370   for (unsigned device=0; device<devices.size(); ++device)
00371   {
00372     updateHeatingConfig(device);
00373   }
00374   return true;
00375 }
00376 
00377 
00378 static struct
00379 {
00380   char *program_name_;
00381   char *interface_;
00382   char *name_;
00383   bool program_;
00384   bool help_;
00385   int device_;
00386   string motor_;
00387   string actuators_;
00388   string board_;
00389   bool update_motor_heating_config_;
00390   bool enforce_heating_model_;
00391 } g_options;
00392 
00393 void Usage(string msg = "")
00394 {
00395   fprintf(stderr, "Usage: %s [options]\n", g_options.program_name_);
00396   fprintf(stderr, " -i, --interface <i>    Use the network interface <i>\n");
00397   fprintf(stderr, " -a, --actuators <file> Get the actuator definitions from file (default: actuators.conf)\n");
00398   fprintf(stderr, " -d, --device <d>       Select the device to program\n");
00399   fprintf(stderr, " -b, --board <b>        Set the expected board type (wg005, wg006, wg021)\n");
00400   fprintf(stderr, " -p, --program          Program a motor control board\n");
00401   fprintf(stderr, " -n, --name <n>         Set the name of the motor control board to <n>\n");
00402   fprintf(stderr, " -U, --update_heating_config Update motor heating model configuration of all boards\n");
00403   fprintf(stderr, "     Known actuator names:\n");
00404   BOOST_FOREACH(ActuatorPair p, actuators)
00405   {
00406     string name = p.first;
00407     fprintf(stderr, "        %s\n", name.c_str());
00408   }
00409   fprintf(stderr, " -m, --motor <m>        Set the configuration for motor <m>\n");
00410   fprintf(stderr, "     Legal motor values are:\n");
00411   BOOST_FOREACH(MotorPair p, motors)
00412   {
00413     const string &name(p.first);
00414     const WG0XActuatorInfo &info(p.second.actuator_info_);
00415     fprintf(stderr, "        %s - %s %s\n", name.c_str(), info.motor_make_, info.motor_model_);
00416   }
00417   fprintf(stderr, " -h, --help    Print this message and exit\n");
00418   if (msg != "")
00419   {
00420     fprintf(stderr, "Error: %s\n", msg.c_str());
00421     exit(-1);
00422   }
00423   else
00424   {
00425     exit(0);
00426   }
00427 }
00428 
00429 
00430 bool getDoubleAttribute(TiXmlElement *params, const char* motor_name, const char* param_name, double& value)
00431 {
00432   const char *val_str = params->Attribute(param_name);
00433   if (val_str == NULL)
00434   {
00435     ROS_ERROR("Attribute '%s' for motor '%s' is not defined", param_name, motor_name);
00436     return false;
00437   }
00438   
00439   char *endptr=NULL;
00440   value = strtod(val_str, &endptr);
00441   if ((endptr == val_str) || (endptr < (val_str+strlen(val_str))))
00442   {
00443     ROS_ERROR("Couldn't convert '%s' to double for attribute '%s' of motor '%s'", 
00444               val_str, param_name, motor_name);
00445     return false;
00446   }
00447 
00448   return true;
00449 }
00450 
00451 bool getIntegerAttribute(TiXmlElement *params, const char* motor_name, const char* param_name, int& value)
00452 {
00453   const char *val_str = params->Attribute(param_name);
00454   if (val_str == NULL)
00455   {
00456     ROS_ERROR("Attribute '%s' for motor '%s' is not defined", param_name, motor_name);
00457     return false;
00458   }
00459   
00460   char *endptr=NULL;
00461   value = strtol(val_str, &endptr, 0);
00462   if ((endptr == val_str) || (endptr < (val_str+strlen(val_str))))
00463   {
00464     ROS_ERROR("Couldn't convert '%s' to integer for attribute '%s' of motor '%s'", 
00465               val_str, param_name, motor_name);
00466     return false;
00467   }
00468 
00469   return true;
00470 }
00471 
00472 bool getStringAttribute(TiXmlElement *params, const char* motor_name, const char* param_name, char* strbuf, unsigned buflen)
00473 {
00474   const char *val = params->Attribute(param_name);
00475   if (val == NULL)
00476   {
00477     ROS_ERROR("No '%s' attribute for motor '%s'", param_name, motor_name);
00478     return false;
00479   }
00480   if (strlen(val) >= buflen)
00481   {
00482     ROS_ERROR("'%s' value '%s' for motor '%s' is too long.  Limit value to %d characters.",
00483               param_name, val, motor_name, buflen-1);
00484     return false;
00485   }
00486   strncpy(strbuf, val, buflen);
00487   strbuf[buflen-1] = '\0';
00488   return true;
00489 }
00490 
00491 
00492 bool parseConfig(TiXmlElement *config)
00493 {
00494   TiXmlElement *actuatorElt = config->FirstChildElement("actuators");
00495   TiXmlElement *motorElt = config->FirstChildElement("motors");
00496 
00497   for (TiXmlElement *elt = actuatorElt->FirstChildElement("actuator");
00498        elt;
00499        elt = elt->NextSiblingElement("actuator"))
00500   {
00501     const char *name = elt->Attribute("name");
00502     if (name == NULL)
00503     {
00504       ROS_ERROR("Acutuator attribute 'name' not specified");
00505       return false;
00506     }
00507 
00508     struct Actuator a;
00509 
00510     const char *motor = elt->Attribute("motor");
00511     if (motor == NULL)
00512     {
00513       ROS_ERROR("For actuator '%s', 'motor' attribute not specified", name);
00514       return false;
00515     }
00516     a.motor = motor;
00517 
00518     const char *board = elt->Attribute("board");
00519     if (board == NULL)
00520     {
00521       ROS_ERROR("For actuator '%s', 'board' attribute not specified", name);
00522       return false;
00523     }
00524     a.board = board;
00525 
00526     const char *enforce_heating_model = elt->Attribute("enforce_heating_model");
00527     if (enforce_heating_model == NULL)
00528     {
00529       // ROS_ERROR("For actuator '%s', 'enforce_heating_model' attribute not specified", name);
00530       //return false;
00531       a.enforce_heating_model=false;
00532     }
00533     else if (strcmp(enforce_heating_model, "true") == 0)
00534     {
00535       a.enforce_heating_model=true;
00536     }
00537     else if (strcmp(enforce_heating_model, "false") == 0)
00538     {
00539       a.enforce_heating_model=false;
00540     }
00541     else 
00542     {
00543       ROS_ERROR("For actuator '%s' : 'enforce_heating_model' attribute should be 'true' or 'false' not '%s'",
00544                 name, enforce_heating_model);
00545       return false;
00546     }
00547 
00548     actuators[name] = a;
00549   }
00550 
00551   WG0XActuatorInfo info;
00552   memset(&info, 0, sizeof(info));
00553   info.major_ = 0;
00554   info.minor_ = 2;
00555   strcpy(info.robot_name_, "PR2");
00556   ethercat_hardware::MotorHeatingModelParametersEepromConfig heating_config;
00557   memset(&heating_config, 0, sizeof(heating_config));
00558   heating_config.major_ = 0;
00559   heating_config.minor_ = 1;
00560   heating_config.enforce_ = 0; // default, don't enforce heating model
00561 
00562   for (TiXmlElement *elt = motorElt->FirstChildElement("motor");
00563        elt;
00564        elt = elt->NextSiblingElement("motor"))
00565   {
00566     bool success = true;
00567     const char *name = elt->Attribute("name");
00568     if (name == NULL)
00569     {
00570       ROS_ERROR("Motor 'name' attribute is not specified");
00571       return false;
00572     }
00573 
00574     TiXmlElement *params = elt->FirstChildElement("params");
00575     if (params == NULL)
00576     {
00577       ROS_ERROR("No 'params' tag available for motor '%s'", name);
00578       return false;
00579     }
00580 
00581     TiXmlElement *encoder = elt->FirstChildElement("encoder");
00582     if (encoder == NULL)
00583     {
00584       ROS_ERROR("No 'encoder' tag available for motor '%s'", name);
00585       return false;
00586     }
00587 
00588     success &= getStringAttribute(params, name, "make", info.motor_make_, sizeof(info.motor_make_));
00589     success &= getStringAttribute(params, name, "model", info.motor_model_, sizeof(info.motor_model_));
00590 
00591     double fvalue;
00592 
00593     success &= getDoubleAttribute(params, name, "max_current", fvalue);
00594     info.max_current_ = fvalue;
00595     success &= getDoubleAttribute(params, name, "speed_constant", fvalue);
00596     info.speed_constant_ = fvalue;
00597     success &= getDoubleAttribute(params, name, "resistance", fvalue);
00598     info.resistance_ = fvalue;
00599     success &= getDoubleAttribute(params, name, "motor_torque_constant", fvalue);
00600     info.motor_torque_constant_ = fvalue;
00601     success &= getDoubleAttribute(encoder, name, "reduction", fvalue);
00602     info.encoder_reduction_ = fvalue;
00603 
00604     int ivalue=-1;
00605     success &= getIntegerAttribute(encoder, name, "pulses_per_revolution", ivalue);
00606     info.pulses_per_revolution_ = ivalue;
00607     
00608     ethercat_hardware::MotorHeatingModelParameters &hmp(heating_config.params_);
00609     success &= getDoubleAttribute(params, name, "housing_to_ambient_thermal_resistance", fvalue);
00610     hmp.housing_to_ambient_thermal_resistance_ = fvalue;
00611     success &= getDoubleAttribute(params, name, "winding_to_housing_thermal_resistance", fvalue);
00612     hmp.winding_to_housing_thermal_resistance_ = fvalue;
00613     success &= getDoubleAttribute(params, name, "winding_thermal_time_constant", fvalue);
00614     hmp.winding_thermal_time_constant_ = fvalue;
00615     success &= getDoubleAttribute(params, name, "housing_thermal_time_constant", fvalue);
00616     hmp.housing_thermal_time_constant_ = fvalue;      
00617     success &= getDoubleAttribute(params, name, "max_winding_temperature", fvalue);
00618     hmp.max_winding_temperature_       = fvalue;    
00619 
00620     if (!success)
00621     {
00622       return false;
00623     }
00624 
00625     if (motors.find(name) != motors.end())
00626     {
00627       ROS_ERROR("Motor named '%s' exists motor than once",name);
00628       return false;
00629     }
00630 
00631     motors[name] = Config(info, heating_config);
00632   }
00633   return true;
00634 }
00635 
00636 
00637 
00638 
00639 int main(int argc, char *argv[])
00640 {
00641 
00642   // Set log level to DEBUG to allow device information to be displayed to
00643   // output by default
00644   log4cxx::LoggerPtr logger = log4cxx::Logger::getLogger(ROSCONSOLE_DEFAULT_NAME);
00645   logger->setLevel(ros::console::g_level_lookup[ros::console::levels::Debug]);
00646   ros::console::notifyLoggerLevelsChanged();
00647   
00648   // Parse options
00649   g_options.program_name_ = argv[0];
00650   g_options.device_ = -1;
00651   g_options.help_ = false;
00652   g_options.board_ = "";
00653   g_options.update_motor_heating_config_ = false;
00654   g_options.enforce_heating_model_ = false;
00655   while (1)
00656   {
00657     static struct option long_options[] = {
00658       {"help", no_argument, 0, 'h'},
00659       {"interface", required_argument, 0, 'i'},
00660       {"name", required_argument, 0, 'n'},
00661       {"device", required_argument, 0, 'd'},
00662       {"board", required_argument, 0, 'b'},
00663       {"motor", required_argument, 0, 'm'},
00664       {"program", no_argument, 0, 'p'},
00665       {"actuators", required_argument, 0, 'a'},
00666       {"update_heating_config", no_argument, 0, 'U'},
00667       {"enforce_heating_model", no_argument, 0, 'H'},
00668     };
00669     int option_index = 0;
00670     int c = getopt_long(argc, argv, "d:b:hi:m:n:pa:UH", long_options, &option_index);
00671     if (c == -1) break;
00672     switch (c)
00673     {
00674       case 'h':
00675         g_options.help_ = true;
00676         break;
00677       case 'd':
00678         g_options.device_ = atoi(optarg);
00679         break;
00680       case 'b':
00681         g_options.board_ = optarg;
00682         break;
00683       case 'i':
00684         g_options.interface_ = optarg;
00685         break;
00686       case 'n':
00687         g_options.name_ = optarg;
00688         break;
00689       case 'm':
00690         g_options.motor_ = optarg;
00691         break;
00692       case 'p':
00693         g_options.program_ = 1;
00694         break;
00695       case 'a':
00696         g_options.actuators_ = optarg;
00697         break;
00698       case 'U':
00699         g_options.update_motor_heating_config_ = true;
00700         break;
00701       case 'H':
00702         g_options.enforce_heating_model_ = true;
00703         break;
00704     }
00705   }
00706 
00707   // Parse configuration file
00708   string filename = "actuators.conf";
00709   if (g_options.actuators_ != "")
00710     filename = g_options.actuators_;
00711   TiXmlDocument xml(filename);
00712 
00713   if (!xml.LoadFile())
00714   {
00715     Usage("Unable to load configuration file");
00716   }
00717 
00718   
00719   if (!parseConfig(xml.RootElement()))
00720   {
00721     exit(EXIT_FAILURE);
00722   }
00723 
00724   if (g_options.help_)
00725     Usage();
00726 
00727   if (optind < argc)
00728   {
00729     Usage("Extra arguments");
00730   }
00731 
00732   if (!g_options.interface_)
00733     Usage("You must specify a network interface");
00734 
00735   // Try to get a raw socket.
00736   int test_sock = socket(PF_PACKET, SOCK_RAW, htons(0x88A4));
00737   if ((test_sock < 0) && (errno == EPERM))
00738   {
00739     ROS_FATAL("Insufficient priviledges to obtain raw socket.  Try running as root.");
00740     exit(-1);
00741   }
00742   close(test_sock);
00743 
00744   // Keep the kernel from swapping us out
00745   if (mlockall(MCL_CURRENT | MCL_FUTURE) != 0)
00746   {
00747     ROS_WARN("mlockall failed : %s", strerror(errno));
00748   }
00749 
00750   init(g_options.interface_);
00751 
00752   if (g_options.update_motor_heating_config_)
00753   {
00754     updateAllHeatingConfig();
00755   }
00756 
00757   if (g_options.program_)
00758   {
00759     string board = "wg005";
00760     bool enforce_heating_model = false;
00761     if (!g_options.name_)
00762       Usage("You must specify a name");
00763     if (g_options.motor_ == "")
00764     {
00765       if (actuators.find(g_options.name_) == actuators.end())
00766         Usage("No default motor for this name");
00767       g_options.motor_ = actuators[g_options.name_].motor;
00768       board = actuators[g_options.name_].board;
00769       enforce_heating_model = actuators[g_options.name_].enforce_heating_model;
00770     }
00771     if (g_options.board_ != "") {
00772       board = g_options.board_;
00773     }
00774     if (g_options.device_ == -1)
00775       Usage("You must specify a device #");
00776     if (motors.find(g_options.motor_) == motors.end())
00777       Usage("You must specify a valid motor");
00778     if (g_options.enforce_heating_model_)
00779       enforce_heating_model = true;
00780 
00781     programDevice(g_options.device_, motors[g_options.motor_], g_options.name_, board, enforce_heating_model);
00782   }
00783 
00784   return 0;
00785 }
00786 
00787 


ethercat_hardware
Author(s): Rob Wheeler , Derek King
autogenerated on Thu Jun 6 2019 19:46:32