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 #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
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
00121 if ((ni = init_ec(interface)) == NULL)
00122 {
00123 fprintf(stderr, "Unable to initialize interface: %s\n", interface);
00124 exit(-1);
00125 }
00126
00127
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
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
00299
00300
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
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
00530
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;
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
00643
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
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
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
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
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