motorconf.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2008, Willow Garage, Inc.
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of the Willow Garage nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  *********************************************************************/
34 
35 #include <map>
36 #include <stdio.h>
37 #include <getopt.h>
38 #include <sys/mman.h>
39 
40 #include <tinyxml.h>
41 
42 #include <ethercat/ethercat_xenomai_drv.h>
43 #include <dll/ethercat_dll.h>
44 #include <al/ethercat_AL.h>
45 #include <al/ethercat_master.h>
46 #include <al/ethercat_slave_handler.h>
47 
49 #include <ethercat_hardware/wg0x.h>
50 #include <ethercat_hardware/wg05.h>
51 #include <ethercat_hardware/wg06.h>
54 
55 #include <boost/foreach.hpp>
56 
57 #include <net/if.h>
58 #include <sys/ioctl.h>
59 #include <netinet/in.h>
60 
61 #include <log4cxx/logger.h>
62 
63 using namespace ethercat_hardware;
64 
65 vector<EthercatDevice *> devices;
66 
67 struct Actuator {
68  string motor;
69  string board;
71 };
72 typedef pair<string, Actuator> ActuatorPair;
73 map<string, Actuator> actuators;
74 
75 struct Config
76 {
77  Config(const WG0XActuatorInfo &actuator_info, const MotorHeatingModelParametersEepromConfig &heating_config) :
78  actuator_info_(actuator_info), heating_config_(heating_config)
79  { }
80  Config() {}
83 };
84 typedef pair<string, Config> MotorPair;
85 map<string, Config> motors;
86 
87 void init(char *interface)
88 {
89  // open temporary socket to use with ioctl
90  int sock = socket(PF_INET, SOCK_DGRAM, 0);
91  if (sock < 0) {
92  int error = errno;
93  fprintf(stderr,"Couldn't open temp socket : %s", strerror(error));
94  exit(-1);
95  }
96 
97  struct ifreq ifr;
98  strncpy(ifr.ifr_name, interface, IFNAMSIZ);
99  if (ioctl(sock, SIOCGIFFLAGS, &ifr) < 0) {
100  int error = errno;
101  fprintf(stderr,"Cannot get interface flags for %s: %s\n", interface, strerror(error));
102  exit(-1);
103  }
104 
105  close(sock);
106  sock = -1;
107 
108  if (!(ifr.ifr_flags & IFF_UP)) {
109  fprintf(stderr,"Interface %s is not UP. Try : ifup %s\n", interface, interface);
110  exit(-1);
111  }
112  if (!(ifr.ifr_flags & IFF_RUNNING)) {
113  fprintf(stderr,"Interface %s is not RUNNING. Is cable plugged in and device powered?\n", interface);
114  exit(-1);
115  }
116 
117  struct netif *ni;
118 
119  // Initialize network interface
120  if ((ni = init_ec(interface)) == NULL)
121  {
122  fprintf(stderr, "Unable to initialize interface: %s\n", interface);
123  exit(-1);
124  }
125 
126  // Initialize Application Layer (AL)
127  EtherCAT_DataLinkLayer::instance()->attach(ni);
128  EtherCAT_AL *al;
129  if ((al = EtherCAT_AL::instance()) == NULL)
130  {
131  fprintf(stderr, "Unable to initialize Application Layer (AL): %p\n", al);
132  exit(-1);
133  }
134 
135  uint32_t num_slaves = al->get_num_slaves();
136  if (num_slaves == 0)
137  {
138  fprintf(stderr, "Unable to locate any slaves\n");
139  exit(-1);
140  }
141 
142  // Initialize Master
143  EtherCAT_Master *em;
144  if ((em = EtherCAT_Master::instance()) == NULL)
145  {
146  fprintf(stderr, "Unable to initialize EtherCAT_Master: %p", em);
147  exit(-1);
148  }
149 
150  static int start_address = 0x00010000;
151 
152  for (unsigned int slave = 0; slave < num_slaves; ++slave)
153  {
154  EC_FixedStationAddress fsa(slave + 1);
155  EtherCAT_SlaveHandler *sh = em->get_slave_handler(fsa);
156  if (sh == NULL)
157  {
158  fprintf(stderr, "Unable to get slave handler #%d", slave);
159  exit(-1);
160  }
161 
162  if (sh->get_product_code() == WG05::PRODUCT_CODE)
163  {
164  WG05 *dev = new WG05();
165  dev->construct(sh, start_address);
166  devices.push_back(dev);
167  }
168  else if (sh->get_product_code() == WG06::PRODUCT_CODE)
169  {
170  WG06 *dev = new WG06();
171  dev->construct(sh, start_address);
172  devices.push_back(dev);
173  }
174  else if (sh->get_product_code() == WG021::PRODUCT_CODE)
175  {
176  WG021 *dev = new WG021();
177  dev->construct(sh, start_address);
178  devices.push_back(dev);
179  }
180  else if (sh->get_product_code() == WG014::PRODUCT_CODE)
181  {
182  WG014 *dev = new WG014();
183  dev->construct(sh, start_address);
184  devices.push_back(dev);
185  }
186  else
187  {
188  devices.push_back(NULL);
189  }
190  }
191 
192  BOOST_FOREACH(EthercatDevice *device, devices)
193  {
194  if (!device) continue;
195  if (!device->sh_->to_state(EC_OP_STATE))
196  {
197  fprintf(stderr, "Unable set device %d into OP_STATE", device->sh_->get_ring_position());
198  }
199  }
200 
201  BOOST_FOREACH(EthercatDevice *device, devices)
202  {
203  if (!device) continue;
204  device->use_ros_ = false;
205  device->initialize(NULL, true);
206  }
207 }
208 
210 {
211  if (dynamic_cast<WG021 *>(d)) {
212  return "wg021";
213  } else if (dynamic_cast<WG06 *>(d)) {
214  return "wg006";
215  } else if (dynamic_cast<WG05 *>(d)) {
216  return "wg005";
217  }
218  return "unknown";
219 }
220 
221 
222 WG0X* getWGDevice(int device)
223 {
224  uint32_t num_slaves = EtherCAT_AL::instance()->get_num_slaves();
225  if ((device >= (int)num_slaves) || (device < 0))
226  {
227  ROS_FATAL("Invalid device number %d. Must be value between 0 and %d", device, num_slaves-1);
228  return NULL;
229  }
230 
231  if (devices[device] == NULL)
232  {
233  ROS_FATAL("There is no device at position #%d", device);
234  return NULL;
235  }
236 
237  WG0X *wg = dynamic_cast<WG0X *>(devices[device]);
238  if (wg==NULL)
239  {
240  ROS_FATAL("The device a position #%d is not programmable", device);
241  return NULL;
242  }
243 
244  return wg;
245 }
246 
247 
248 bool programDevice(int device,
249  const Config &config,
250  char *name, string expected_board, bool enforce_heating_model)
251 {
252  WG0X *wg = getWGDevice(device);
253  if (wg == NULL)
254  {
255  return false;
256  }
257 
258  string board = boardName(devices[device]);
259  if (expected_board != board)
260  {
261  ROS_FATAL("Device #%02d is a %s, but %s expects a %s\n", device, board.c_str(), name, expected_board.c_str());
262  return false;
263  }
264  ROS_INFO("Programming device %d, to be named: %s\n", device, name);
265 
266  WG0XActuatorInfo actuator_info = config.actuator_info_;
267  if (strlen(name) >= sizeof(actuator_info.name_))
268  {
269  ROS_FATAL("Device name '%s' is too long", name);
270  }
271  strncpy(actuator_info.name_, name, sizeof(actuator_info.name_));
272  actuator_info.generateCRC();
273 
274  ROS_INFO("Programming actuator version %d.%d",
275  int(actuator_info.major_), int(actuator_info.minor_));
276 
277  EthercatDirectCom com(EtherCAT_DataLinkLayer::instance());
278  if (!wg->program(&com, actuator_info))
279  {
280  ROS_FATAL("Error writing actuator info to device #%d", device);
281  return false;
282  }
283 
285  heating_config.enforce_ = enforce_heating_model;
286  heating_config.generateCRC();
287  if (!wg->program(&com, config.heating_config_))
288  {
289  ROS_FATAL("Writing heating model config to device #%d", device);
290  return false;
291  }
292 
293  return true;
294 }
295 
296 
297 // Uses ActuatorInfo name already stored in device
298 // to find appropriate motor heating model config
299 // Then only updates motor heating model config
300 bool updateHeatingConfig(int device)
301 {
302  WG0XActuatorInfo actuator_info;
303  EthercatDirectCom com(EtherCAT_DataLinkLayer::instance());
304  WG0X *wg = getWGDevice(device);
305  if (wg == NULL)
306  {
307  ROS_WARN("Skipping update of device %d", device);
308  return false;
309  }
310 
311  if (!wg->readActuatorInfoFromEeprom(&com, actuator_info))
312  {
313  ROS_ERROR("Could not read actuator info from device %d", device);
314  return false;
315  }
316 
317  if (!actuator_info.verifyCRC())
318  {
319  ROS_ERROR("Device %d has not actuator configuration", device);
320  return false;
321  }
322 
323  if (actuators.find(actuator_info.name_) == actuators.end())
324  {
325  ROS_ERROR("Could not find actuator info for device %d with name '%s'",
326  device, actuator_info.name_);
327  return false;
328  }
329 
330  const Actuator &actuator(actuators[actuator_info.name_]);
331  const string &motor_name(actuator.motor);
332  bool enforce_heating_model = actuator.enforce_heating_model;
333 
334  if (motors.find(motor_name) == motors.end())
335  {
336  ROS_ERROR("Could not find motor '%s' for device %d with actuator name '%s'",
337  motor_name.c_str(), device, actuator_info.name_);
338  return false;
339  }
340 
341  const Config &config(motors[motor_name]);
342 
343  if (strcmp(config.actuator_info_.motor_model_, actuator_info.motor_model_) != 0)
344  {
345  ROS_ERROR("For device %d '%s' : the motor name stored in EEPROM '%s' does not match the motor name '%s' from XML file",
346  device, actuator_info.name_, actuator_info.motor_model_, config.actuator_info_.motor_model_);
347  return false;
348  }
349 
351  heating_config.enforce_ = enforce_heating_model;
352  heating_config.generateCRC();
353  if (!wg->program(&com, heating_config))
354  {
355  ROS_FATAL("Writing heating model config to device #%d", device);
356  return false;
357  }
358 
359  ROS_INFO("Updated device %d (%s) with heating config for motor '%s'",
360  device, actuator_info.name_, config.actuator_info_.motor_model_);
361 
362  return true;
363 }
364 
365 
366 // Updates heating configuration of all devices
368 {
369  for (unsigned device=0; device<devices.size(); ++device)
370  {
371  updateHeatingConfig(device);
372  }
373  return true;
374 }
375 
376 
377 static struct
378 {
380  char *interface_;
381  char *name_;
382  bool program_;
383  bool help_;
384  int device_;
385  string motor_;
386  string actuators_;
387  string board_;
390 } g_options;
391 
392 void Usage(string msg = "")
393 {
394  fprintf(stderr, "Usage: %s [options]\n", g_options.program_name_);
395  fprintf(stderr, " -i, --interface <i> Use the network interface <i>\n");
396  fprintf(stderr, " -a, --actuators <file> Get the actuator definitions from file (default: actuators.conf)\n");
397  fprintf(stderr, " -d, --device <d> Select the device to program\n");
398  fprintf(stderr, " -b, --board <b> Set the expected board type (wg005, wg006, wg021)\n");
399  fprintf(stderr, " -p, --program Program a motor control board\n");
400  fprintf(stderr, " -n, --name <n> Set the name of the motor control board to <n>\n");
401  fprintf(stderr, " -U, --update_heating_config Update motor heating model configuration of all boards\n");
402  fprintf(stderr, " Known actuator names:\n");
403  BOOST_FOREACH(ActuatorPair p, actuators)
404  {
405  string name = p.first;
406  fprintf(stderr, " %s\n", name.c_str());
407  }
408  fprintf(stderr, " -m, --motor <m> Set the configuration for motor <m>\n");
409  fprintf(stderr, " Legal motor values are:\n");
410  BOOST_FOREACH(MotorPair p, motors)
411  {
412  const string &name(p.first);
413  const WG0XActuatorInfo &info(p.second.actuator_info_);
414  fprintf(stderr, " %s - %s %s\n", name.c_str(), info.motor_make_, info.motor_model_);
415  }
416  fprintf(stderr, " -h, --help Print this message and exit\n");
417  if (msg != "")
418  {
419  fprintf(stderr, "Error: %s\n", msg.c_str());
420  exit(-1);
421  }
422  else
423  {
424  exit(0);
425  }
426 }
427 
428 
429 bool getDoubleAttribute(TiXmlElement *params, const char* motor_name, const char* param_name, double& value)
430 {
431  const char *val_str = params->Attribute(param_name);
432  if (val_str == NULL)
433  {
434  ROS_ERROR("Attribute '%s' for motor '%s' is not defined", param_name, motor_name);
435  return false;
436  }
437 
438  char *endptr=NULL;
439  value = strtod(val_str, &endptr);
440  if ((endptr == val_str) || (endptr < (val_str+strlen(val_str))))
441  {
442  ROS_ERROR("Couldn't convert '%s' to double for attribute '%s' of motor '%s'",
443  val_str, param_name, motor_name);
444  return false;
445  }
446 
447  return true;
448 }
449 
450 bool getIntegerAttribute(TiXmlElement *params, const char* motor_name, const char* param_name, int& value)
451 {
452  const char *val_str = params->Attribute(param_name);
453  if (val_str == NULL)
454  {
455  ROS_ERROR("Attribute '%s' for motor '%s' is not defined", param_name, motor_name);
456  return false;
457  }
458 
459  char *endptr=NULL;
460  value = strtol(val_str, &endptr, 0);
461  if ((endptr == val_str) || (endptr < (val_str+strlen(val_str))))
462  {
463  ROS_ERROR("Couldn't convert '%s' to integer for attribute '%s' of motor '%s'",
464  val_str, param_name, motor_name);
465  return false;
466  }
467 
468  return true;
469 }
470 
471 bool getStringAttribute(TiXmlElement *params, const char* motor_name, const char* param_name, char* strbuf, unsigned buflen)
472 {
473  const char *val = params->Attribute(param_name);
474  if (val == NULL)
475  {
476  ROS_ERROR("No '%s' attribute for motor '%s'", param_name, motor_name);
477  return false;
478  }
479  if (strlen(val) >= buflen)
480  {
481  ROS_ERROR("'%s' value '%s' for motor '%s' is too long. Limit value to %d characters.",
482  param_name, val, motor_name, buflen-1);
483  return false;
484  }
485  strncpy(strbuf, val, buflen);
486  strbuf[buflen-1] = '\0';
487  return true;
488 }
489 
490 
491 bool parseConfig(TiXmlElement *config)
492 {
493  TiXmlElement *actuatorElt = config->FirstChildElement("actuators");
494  TiXmlElement *motorElt = config->FirstChildElement("motors");
495 
496  for (TiXmlElement *elt = actuatorElt->FirstChildElement("actuator");
497  elt;
498  elt = elt->NextSiblingElement("actuator"))
499  {
500  const char *name = elt->Attribute("name");
501  if (name == NULL)
502  {
503  ROS_ERROR("Acutuator attribute 'name' not specified");
504  return false;
505  }
506 
507  struct Actuator a;
508 
509  const char *motor = elt->Attribute("motor");
510  if (motor == NULL)
511  {
512  ROS_ERROR("For actuator '%s', 'motor' attribute not specified", name);
513  return false;
514  }
515  a.motor = motor;
516 
517  const char *board = elt->Attribute("board");
518  if (board == NULL)
519  {
520  ROS_ERROR("For actuator '%s', 'board' attribute not specified", name);
521  return false;
522  }
523  a.board = board;
524 
525  const char *enforce_heating_model = elt->Attribute("enforce_heating_model");
526  if (enforce_heating_model == NULL)
527  {
528  // ROS_ERROR("For actuator '%s', 'enforce_heating_model' attribute not specified", name);
529  //return false;
530  a.enforce_heating_model=false;
531  }
532  else if (strcmp(enforce_heating_model, "true") == 0)
533  {
534  a.enforce_heating_model=true;
535  }
536  else if (strcmp(enforce_heating_model, "false") == 0)
537  {
538  a.enforce_heating_model=false;
539  }
540  else
541  {
542  ROS_ERROR("For actuator '%s' : 'enforce_heating_model' attribute should be 'true' or 'false' not '%s'",
543  name, enforce_heating_model);
544  return false;
545  }
546 
547  actuators[name] = a;
548  }
549 
550  WG0XActuatorInfo info;
551  memset(&info, 0, sizeof(info));
552  info.major_ = 0;
553  info.minor_ = 2;
554  strcpy(info.robot_name_, "PR2");
556  memset(&heating_config, 0, sizeof(heating_config));
557  heating_config.major_ = 0;
558  heating_config.minor_ = 1;
559  heating_config.enforce_ = 0; // default, don't enforce heating model
560 
561  for (TiXmlElement *elt = motorElt->FirstChildElement("motor");
562  elt;
563  elt = elt->NextSiblingElement("motor"))
564  {
565  bool success = true;
566  const char *name = elt->Attribute("name");
567  if (name == NULL)
568  {
569  ROS_ERROR("Motor 'name' attribute is not specified");
570  return false;
571  }
572 
573  TiXmlElement *params = elt->FirstChildElement("params");
574  if (params == NULL)
575  {
576  ROS_ERROR("No 'params' tag available for motor '%s'", name);
577  return false;
578  }
579 
580  TiXmlElement *encoder = elt->FirstChildElement("encoder");
581  if (encoder == NULL)
582  {
583  ROS_ERROR("No 'encoder' tag available for motor '%s'", name);
584  return false;
585  }
586 
587  success &= getStringAttribute(params, name, "make", info.motor_make_, sizeof(info.motor_make_));
588  success &= getStringAttribute(params, name, "model", info.motor_model_, sizeof(info.motor_model_));
589 
590  double fvalue;
591 
592  success &= getDoubleAttribute(params, name, "max_current", fvalue);
593  info.max_current_ = fvalue;
594  success &= getDoubleAttribute(params, name, "speed_constant", fvalue);
595  info.speed_constant_ = fvalue;
596  success &= getDoubleAttribute(params, name, "resistance", fvalue);
597  info.resistance_ = fvalue;
598  success &= getDoubleAttribute(params, name, "motor_torque_constant", fvalue);
599  info.motor_torque_constant_ = fvalue;
600  success &= getDoubleAttribute(encoder, name, "reduction", fvalue);
601  info.encoder_reduction_ = fvalue;
602 
603  int ivalue=-1;
604  success &= getIntegerAttribute(encoder, name, "pulses_per_revolution", ivalue);
605  info.pulses_per_revolution_ = ivalue;
606 
608  success &= getDoubleAttribute(params, name, "housing_to_ambient_thermal_resistance", fvalue);
610  success &= getDoubleAttribute(params, name, "winding_to_housing_thermal_resistance", fvalue);
612  success &= getDoubleAttribute(params, name, "winding_thermal_time_constant", fvalue);
613  hmp.winding_thermal_time_constant_ = fvalue;
614  success &= getDoubleAttribute(params, name, "housing_thermal_time_constant", fvalue);
615  hmp.housing_thermal_time_constant_ = fvalue;
616  success &= getDoubleAttribute(params, name, "max_winding_temperature", fvalue);
617  hmp.max_winding_temperature_ = fvalue;
618 
619  if (!success)
620  {
621  return false;
622  }
623 
624  if (motors.find(name) != motors.end())
625  {
626  ROS_ERROR("Motor named '%s' exists motor than once",name);
627  return false;
628  }
629 
630  motors[name] = Config(info, heating_config);
631  }
632  return true;
633 }
634 
635 
636 
637 
638 int main(int argc, char *argv[])
639 {
640 
641  // Set log level to DEBUG to allow device information to be displayed to
642  // output by default
643  log4cxx::LoggerPtr logger = log4cxx::Logger::getLogger(ROSCONSOLE_DEFAULT_NAME);
644  logger->setLevel(ros::console::g_level_lookup[ros::console::levels::Debug]);
646 
647  // Parse options
648  g_options.program_name_ = argv[0];
649  g_options.device_ = -1;
650  g_options.help_ = false;
651  g_options.board_ = "";
652  g_options.update_motor_heating_config_ = false;
653  g_options.enforce_heating_model_ = false;
654  while (1)
655  {
656  static struct option long_options[] = {
657  {"help", no_argument, 0, 'h'},
658  {"interface", required_argument, 0, 'i'},
659  {"name", required_argument, 0, 'n'},
660  {"device", required_argument, 0, 'd'},
661  {"board", required_argument, 0, 'b'},
662  {"motor", required_argument, 0, 'm'},
663  {"program", no_argument, 0, 'p'},
664  {"actuators", required_argument, 0, 'a'},
665  {"update_heating_config", no_argument, 0, 'U'},
666  {"enforce_heating_model", no_argument, 0, 'H'},
667  };
668  int option_index = 0;
669  int c = getopt_long(argc, argv, "d:b:hi:m:n:pa:UH", long_options, &option_index);
670  if (c == -1) break;
671  switch (c)
672  {
673  case 'h':
674  g_options.help_ = true;
675  break;
676  case 'd':
677  g_options.device_ = atoi(optarg);
678  break;
679  case 'b':
680  g_options.board_ = optarg;
681  break;
682  case 'i':
683  g_options.interface_ = optarg;
684  break;
685  case 'n':
686  g_options.name_ = optarg;
687  break;
688  case 'm':
689  g_options.motor_ = optarg;
690  break;
691  case 'p':
692  g_options.program_ = 1;
693  break;
694  case 'a':
695  g_options.actuators_ = optarg;
696  break;
697  case 'U':
698  g_options.update_motor_heating_config_ = true;
699  break;
700  case 'H':
701  g_options.enforce_heating_model_ = true;
702  break;
703  }
704  }
705 
706  // Parse configuration file
707  string filename = "actuators.conf";
708  if (g_options.actuators_ != "")
709  filename = g_options.actuators_;
710  TiXmlDocument xml(filename);
711 
712  if (!xml.LoadFile())
713  {
714  Usage("Unable to load configuration file");
715  }
716 
717 
718  if (!parseConfig(xml.RootElement()))
719  {
720  exit(EXIT_FAILURE);
721  }
722 
723  if (g_options.help_)
724  Usage();
725 
726  if (optind < argc)
727  {
728  Usage("Extra arguments");
729  }
730 
731  if (!g_options.interface_)
732  Usage("You must specify a network interface");
733 
734  // Try to get a raw socket.
735  int test_sock = socket(PF_PACKET, SOCK_RAW, htons(0x88A4));
736  if ((test_sock < 0) && (errno == EPERM))
737  {
738  ROS_FATAL("Insufficient priviledges to obtain raw socket. Try running as root.");
739  exit(-1);
740  }
741  close(test_sock);
742 
743  // Keep the kernel from swapping us out
744  if (mlockall(MCL_CURRENT | MCL_FUTURE) != 0)
745  {
746  ROS_WARN("mlockall failed : %s", strerror(errno));
747  }
748 
749  init(g_options.interface_);
750 
751  if (g_options.update_motor_heating_config_)
752  {
754  }
755 
756  if (g_options.program_)
757  {
758  string board = "wg005";
759  bool enforce_heating_model = false;
760  if (!g_options.name_)
761  Usage("You must specify a name");
762  if (g_options.motor_ == "")
763  {
764  if (actuators.find(g_options.name_) == actuators.end())
765  Usage("No default motor for this name");
766  g_options.motor_ = actuators[g_options.name_].motor;
767  board = actuators[g_options.name_].board;
768  enforce_heating_model = actuators[g_options.name_].enforce_heating_model;
769  }
770  if (g_options.board_ != "") {
771  board = g_options.board_;
772  }
773  if (g_options.device_ == -1)
774  Usage("You must specify a device #");
775  if (motors.find(g_options.motor_) == motors.end())
776  Usage("You must specify a valid motor");
777  if (g_options.enforce_heating_model_)
778  enforce_heating_model = true;
779 
780  programDevice(g_options.device_, motors[g_options.motor_], g_options.name_, board, enforce_heating_model);
781  }
782 
783  return 0;
784 }
785 
786 
msg
char name_[64]
Definition: wg0x.h:141
Config()
Definition: motorconf.cpp:80
uint16_t major_
Major revision of this structure.
MotorHeatingModelParameters params_
Motor parameters.
#define ROS_FATAL(...)
double winding_thermal_time_constant_
Thermal time constant of motor winding : in seconds.
uint32_t pulses_per_revolution_
Definition: wg0x.h:150
void construct(EtherCAT_SlaveHandler *sh, int &start_address)
< Construct EtherCAT device
Definition: wg06.cpp:90
static bool getDoubleAttribute(TiXmlElement *elt, const std::string &filename, const char *param_name, double &value)
bool enforce_heating_model
Definition: motorconf.cpp:70
string board_
Definition: motorconf.cpp:387
ROSCONSOLE_DECL void notifyLoggerLevelsChanged()
char * interface_
Definition: motorconf.cpp:380
bool readActuatorInfoFromEeprom(EthercatCom *com, WG0XActuatorInfo &actuator_info)
Reads actuator info from eeprom.
Definition: wg0x.cpp:999
void construct(EtherCAT_SlaveHandler *sh, int &start_address)
< Construct EtherCAT device
Definition: wg021.cpp:53
string actuators_
Definition: motorconf.cpp:386
double speed_constant_
Definition: wg0x.h:146
char motor_model_[32]
Definition: wg0x.h:144
double max_current_
Definition: wg0x.h:145
bool updateAllHeatingConfig()
Definition: motorconf.cpp:367
virtual int initialize(pr2_hardware_interface::HardwareInterface *, bool allow_unprogrammed=0)=0
char robot_name_[32]
Definition: wg0x.h:142
EtherCAT driver for WG005 motor controller.
Definition: wg05.h:43
static struct @52 g_options
WG0X * getWGDevice(int device)
Definition: motorconf.cpp:222
bool enforce_
0 if heating model should be not be enforced, 0 otherwise
void generateCRC(void)
Calculate CRC of structure and update crc32_256_ and crc32_264_ elements.
Definition: wg0x.cpp:146
string motor
Definition: motorconf.cpp:68
int device_
Definition: motorconf.cpp:384
static bool getStringAttribute(TiXmlElement *elt, const std::string &filename, const char *param_name, std::string &value)
void construct(EtherCAT_SlaveHandler *sh, int &start_address)
< Construct EtherCAT device
Definition: wg014.cpp:42
bool parseConfig(TiXmlElement *config)
Definition: motorconf.cpp:491
double resistance_
Definition: wg0x.h:147
bool program(EthercatCom *com, const WG0XActuatorInfo &actutor_info)
Programs acutator and heating parameters into device EEPROM.
Definition: wg0x.cpp:1051
pair< string, Actuator > ActuatorPair
Definition: motorconf.cpp:72
pair< string, Config > MotorPair
Definition: motorconf.cpp:84
#define ROS_WARN(...)
bool verifyCRC(void) const
Verify CRC stored in actuator info structure.
Definition: wg0x.cpp:131
int main(int argc, char *argv[])
Definition: motorconf.cpp:638
uint16_t minor_
Minor revision of this structure.
double motor_torque_constant_
Definition: wg0x.h:148
double encoder_reduction_
Definition: wg0x.h:149
MotorHeatingModelParametersEepromConfig heating_config_
Definition: motorconf.cpp:82
string boardName(EthercatDevice *d)
Definition: motorconf.cpp:209
void Usage(string msg="")
Definition: motorconf.cpp:392
vector< EthercatDevice * > devices
Definition: motorconf.cpp:65
double winding_to_housing_thermal_resistance_
Thermal resistance between motor winding and motor housing : in C/Watt.
#define ROS_INFO(...)
string motor_
Definition: motorconf.cpp:385
Config(const WG0XActuatorInfo &actuator_info, const MotorHeatingModelParametersEepromConfig &heating_config)
Definition: motorconf.cpp:77
Definition: wg021.h:85
bool programDevice(int device, const Config &config, char *name, string expected_board, bool enforce_heating_model)
Definition: motorconf.cpp:248
bool program_
Definition: motorconf.cpp:382
double housing_thermal_time_constant_
Thermal time constant of motor housing : in seconds.
uint16_t major_
Definition: wg0x.h:138
uint16_t minor_
Definition: wg0x.h:139
static bool getIntegerAttribute(TiXmlElement *elt, const std::string &filename, const char *param_name, int &value)
EtherCAT_SlaveHandler * sh_
map< string, Actuator > actuators
Definition: motorconf.cpp:73
double max_winding_temperature_
temperature limit of motor windings : in Celcius
bool updateHeatingConfig(int device)
Definition: motorconf.cpp:300
char * name_
Definition: motorconf.cpp:381
bool help_
Definition: motorconf.cpp:383
string board
Definition: motorconf.cpp:69
double housing_to_ambient_thermal_resistance_
Thermal resistance between motor housing and ambient : in C/Watt.
Definition: wg0x.h:234
bool enforce_heating_model_
Definition: motorconf.cpp:389
bool update_motor_heating_config_
Definition: motorconf.cpp:388
WG0XActuatorInfo actuator_info_
Definition: motorconf.cpp:81
#define ROS_ERROR(...)
#define ROSCONSOLE_DEFAULT_NAME
char * program_name_
Definition: motorconf.cpp:379
void construct(EtherCAT_SlaveHandler *sh, int &start_address)
< Construct EtherCAT device
Definition: wg05.cpp:52
Definition: wg014.h:40
void init(char *interface)
Definition: motorconf.cpp:87
map< string, Config > motors
Definition: motorconf.cpp:85
char motor_make_[32]
Definition: wg0x.h:143
Definition: wg06.h:163


ethercat_hardware
Author(s): Rob Wheeler , Derek King
autogenerated on Thu Mar 4 2021 03:10:21