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 <ethercat/ethercat_xenomai_drv.h>
00041 #include <dll/ethercat_dll.h>
00042 #include <al/ethercat_AL.h>
00043 #include <al/ethercat_master.h>
00044 #include <al/ethercat_slave_handler.h>
00045
00046 #include <ethercat_hardware/wg0x.h>
00047 #include <ethercat_hardware/wg014.h>
00048
00049 #include <boost/crc.hpp>
00050 #include <boost/foreach.hpp>
00051
00052 #include <net/if.h>
00053 #include <sys/ioctl.h>
00054 #include <netinet/in.h>
00055
00056 vector<EthercatDevice *> devices;
00057
00058 struct Actuator {
00059 string motor;
00060 string board;
00061 };
00062 typedef pair<string, Actuator> ActuatorPair;
00063 map<string, Actuator> actuators;
00064
00065 typedef pair<string, WG0XActuatorInfo> MotorPair;
00066 map<string, WG0XActuatorInfo> motors;
00067
00068 void init(char *interface)
00069 {
00070
00071 int sock = socket(PF_INET, SOCK_DGRAM, 0);
00072 if (sock < 0) {
00073 int error = errno;
00074 fprintf(stderr,"Couldn't open temp socket : %s", strerror(error));
00075 exit(-1);
00076 }
00077
00078 struct ifreq ifr;
00079 strncpy(ifr.ifr_name, interface, IFNAMSIZ);
00080 if (ioctl(sock, SIOCGIFFLAGS, &ifr) < 0) {
00081 int error = errno;
00082 fprintf(stderr,"Cannot get interface flags for %s: %s\n", interface, strerror(error));
00083 exit(-1);
00084 }
00085
00086 close(sock);
00087 sock = -1;
00088
00089 if (!(ifr.ifr_flags & IFF_UP)) {
00090 fprintf(stderr,"Interface %s is not UP. Try : ifup %s\n", interface, interface);
00091 exit(-1);
00092 }
00093 if (!(ifr.ifr_flags & IFF_RUNNING)) {
00094 fprintf(stderr,"Interface %s is not RUNNING. Is cable plugged in and device powered?\n", interface);
00095 exit(-1);
00096 }
00097
00098 struct netif *ni;
00099
00100
00101 if ((ni = init_ec(interface)) == NULL)
00102 {
00103 fprintf(stderr, "Unable to initialize interface: %s\n", interface);
00104 exit(-1);
00105 }
00106
00107
00108 EtherCAT_DataLinkLayer::instance()->attach(ni);
00109 EtherCAT_AL *al;
00110 if ((al = EtherCAT_AL::instance()) == NULL)
00111 {
00112 fprintf(stderr, "Unable to initialize Application Layer (AL): %p\n", al);
00113 exit(-1);
00114 }
00115
00116 uint32_t num_slaves = al->get_num_slaves();
00117 if (num_slaves == 0)
00118 {
00119 fprintf(stderr, "Unable to locate any slaves\n");
00120 exit(-1);
00121 }
00122
00123
00124 EtherCAT_Master *em;
00125 if ((em = EtherCAT_Master::instance()) == NULL)
00126 {
00127 fprintf(stderr, "Unable to initialize EtherCAT_Master: %p", em);
00128 exit(-1);
00129 }
00130
00131 static int start_address = 0x00010000;
00132
00133 for (unsigned int slave = 0; slave < num_slaves; ++slave)
00134 {
00135 EC_FixedStationAddress fsa(slave + 1);
00136 EtherCAT_SlaveHandler *sh = em->get_slave_handler(fsa);
00137 if (sh == NULL)
00138 {
00139 fprintf(stderr, "Unable to get slave handler #%d", slave);
00140 exit(-1);
00141 }
00142
00143 if (sh->get_product_code() == WG05::PRODUCT_CODE)
00144 {
00145 WG05 *dev = new WG05();
00146 dev->construct(sh, start_address);
00147 devices.push_back(dev);
00148 }
00149 else if (sh->get_product_code() == WG06::PRODUCT_CODE)
00150 {
00151 WG06 *dev = new WG06();
00152 dev->construct(sh, start_address);
00153 devices.push_back(dev);
00154 }
00155 else if (sh->get_product_code() == WG021::PRODUCT_CODE)
00156 {
00157 WG021 *dev = new WG021();
00158 dev->construct(sh, start_address);
00159 devices.push_back(dev);
00160 }
00161 else if (sh->get_product_code() == WG014::PRODUCT_CODE)
00162 {
00163 WG014 *dev = new WG014();
00164 dev->construct(sh, start_address);
00165 devices.push_back(dev);
00166 }
00167 else
00168 {
00169 devices.push_back(NULL);
00170 }
00171 }
00172
00173 BOOST_FOREACH(EthercatDevice *device, devices)
00174 {
00175 if (!device) continue;
00176 if (!device->sh_->to_state(EC_OP_STATE))
00177 {
00178 fprintf(stderr, "Unable set device %d into OP_STATE", device->sh_->get_ring_position());
00179 }
00180 }
00181
00182 BOOST_FOREACH(EthercatDevice *device, devices)
00183 {
00184 if (!device) continue;
00185 device->use_ros_ = false;
00186 device->initialize(NULL, true);
00187 }
00188 }
00189
00190 string boardName(EthercatDevice *d)
00191 {
00192 if (dynamic_cast<WG021 *>(d)) {
00193 return "wg021";
00194 } else if (dynamic_cast<WG06 *>(d)) {
00195 return "wg006";
00196 } else if (dynamic_cast<WG05 *>(d)) {
00197 return "wg005";
00198 }
00199 return "unknown";
00200 }
00201
00202 void programDevice(int device, WG0XActuatorInfo &config, char *name, string expected_board)
00203 {
00204 uint32_t num_slaves = EtherCAT_AL::instance()->get_num_slaves();
00205 if ((device >= (int)num_slaves) || (device < 0)) {
00206 ROS_FATAL("Invalid device number %d. Must be value between 0 and %d", device, num_slaves-1);
00207 return;
00208 }
00209
00210 if (devices[device])
00211 {
00212 WG0X *wg = dynamic_cast<WG0X *>(devices[device]);
00213
00214 if (wg) {
00215 string board = boardName(devices[device]);
00216 if (expected_board != board) {
00217 ROS_FATAL("Device #%02d is a %s, but %s expects a %s\n", device, board.c_str(), name, expected_board.c_str());
00218 return;
00219 }
00220 ROS_INFO("Programming device %d, to be named: %s\n", device, name);
00221 strcpy(config.name_, name);
00222 boost::crc_32_type crc32;
00223 crc32.process_bytes(&config, offsetof(WG0XActuatorInfo, crc32_256_));
00224 config.crc32_256_ = crc32.checksum();
00225 crc32.reset();
00226 crc32.process_bytes(&config, offsetof(WG0XActuatorInfo, crc32_264_));
00227 config.crc32_264_ = crc32.checksum();
00228 wg->program(&config);
00229 }
00230 else
00231 {
00232 ROS_FATAL("The device a position #%d is not programmable", device);
00233 }
00234 }
00235 else
00236 {
00237 ROS_FATAL("There is no device at position #%d", device);
00238 }
00239 }
00240
00241 static struct
00242 {
00243 char *program_name_;
00244 char *interface_;
00245 char *name_;
00246 bool program_;
00247 bool help_;
00248 int device_;
00249 string motor_;
00250 string actuators_;
00251 string board_;
00252 } g_options;
00253
00254 void Usage(string msg = "")
00255 {
00256 fprintf(stderr, "Usage: %s [options]\n", g_options.program_name_);
00257 fprintf(stderr, " -i, --interface <i> Use the network interface <i>\n");
00258 fprintf(stderr, " -a, --actuators <file> Get the actuator definitions from file (default: actuators.conf)\n");
00259 fprintf(stderr, " -d, --device <d> Select the device to program\n");
00260 fprintf(stderr, " -b, --board <b> Set the expected board type (wg005, wg006, wg021)\n");
00261 fprintf(stderr, " -p, --program Program a motor control board\n");
00262 fprintf(stderr, " -n, --name <n> Set the name of the motor control board to <n>\n");
00263 fprintf(stderr, " Known actuator names:\n");
00264 BOOST_FOREACH(ActuatorPair p, actuators)
00265 {
00266 string name = p.first;
00267 fprintf(stderr, " %s\n", name.c_str());
00268 }
00269 fprintf(stderr, " -m, --motor <m> Set the configuration for motor <m>\n");
00270 fprintf(stderr, " Legal motor values are:\n");
00271 BOOST_FOREACH(MotorPair p, motors)
00272 {
00273 string name = p.first;
00274 WG0XActuatorInfo info = p.second;
00275 fprintf(stderr, " %s - %s %s\n", name.c_str(), info.motor_make_, info.motor_model_);
00276 }
00277 fprintf(stderr, " -h, --help Print this message and exit\n");
00278 if (msg != "")
00279 {
00280 fprintf(stderr, "Error: %s\n", msg.c_str());
00281 exit(-1);
00282 }
00283 else
00284 {
00285 exit(0);
00286 }
00287 }
00288
00289 void parseConfig(TiXmlElement *config)
00290 {
00291 TiXmlElement *actuatorElt = config->FirstChildElement("actuators");
00292 TiXmlElement *motorElt = config->FirstChildElement("motors");
00293
00294 for (TiXmlElement *elt = actuatorElt->FirstChildElement("actuator");
00295 elt;
00296 elt = elt->NextSiblingElement("actuator"))
00297 {
00298 const char *name = elt->Attribute("name");
00299 struct Actuator a;
00300 a.motor = elt->Attribute("motor");
00301 a.board = elt->Attribute("board");
00302 actuators[name] = a;
00303 }
00304
00305 WG0XActuatorInfo info;
00306 memset(&info, 0, sizeof(info));
00307 info.minor_ = 2;
00308 strcpy(info.robot_name_, "PR2");
00309 for (TiXmlElement *elt = motorElt->FirstChildElement("motor");
00310 elt;
00311 elt = elt->NextSiblingElement("motor"))
00312 {
00313 const char *name = elt->Attribute("name");
00314 TiXmlElement *params = elt->FirstChildElement("params");
00315 TiXmlElement *encoder = elt->FirstChildElement("encoder");
00316
00317 strcpy(info.motor_make_, params->Attribute("make"));
00318 strcpy(info.motor_model_, params->Attribute("model"));
00319
00320 info.max_current_ = atof(params->Attribute("max_current"));
00321 info.speed_constant_ = atof(params->Attribute("speed_constant"));
00322 info.resistance_ = atof(params->Attribute("resistance"));
00323 info.motor_torque_constant_ = atof(params->Attribute("motor_torque_constant"));
00324
00325 info.pulses_per_revolution_ = atoi(encoder->Attribute("pulses_per_revolution"));
00326 info.encoder_reduction_ = atoi(encoder->Attribute("reduction"));
00327
00328 motors[name] = info;
00329 }
00330 }
00331
00332 int main(int argc, char *argv[])
00333 {
00334
00335
00336
00337 log4cxx::LoggerPtr logger = log4cxx::Logger::getLogger(ROSCONSOLE_DEFAULT_NAME);
00338 logger->setLevel(ros::console::g_level_lookup[ros::console::levels::Debug]);
00339 ros::console::notifyLoggerLevelsChanged();
00340
00341
00342 g_options.program_name_ = argv[0];
00343 g_options.device_ = -1;
00344 g_options.help_ = false;
00345 g_options.board_ = "";
00346 while (1)
00347 {
00348 static struct option long_options[] = {
00349 {"help", no_argument, 0, 'h'},
00350 {"interface", required_argument, 0, 'i'},
00351 {"name", required_argument, 0, 'n'},
00352 {"device", required_argument, 0, 'd'},
00353 {"board", required_argument, 0, 'b'},
00354 {"motor", required_argument, 0, 'm'},
00355 {"program", no_argument, 0, 'p'},
00356 {"actuators", required_argument, 0, 'a'},
00357 };
00358 int option_index = 0;
00359 int c = getopt_long(argc, argv, "d:b:hi:m:n:pa:", long_options, &option_index);
00360 if (c == -1) break;
00361 switch (c)
00362 {
00363 case 'h':
00364 g_options.help_ = true;
00365 break;
00366 case 'd':
00367 g_options.device_ = atoi(optarg);
00368 break;
00369 case 'b':
00370 g_options.board_ = optarg;
00371 break;
00372 case 'i':
00373 g_options.interface_ = optarg;
00374 break;
00375 case 'n':
00376 g_options.name_ = optarg;
00377 break;
00378 case 'm':
00379 g_options.motor_ = optarg;
00380 break;
00381 case 'p':
00382 g_options.program_ = 1;
00383 break;
00384 case 'a':
00385 g_options.actuators_ = optarg;
00386 break;
00387 }
00388 }
00389
00390
00391 string filename = "actuators.conf";
00392 if (g_options.actuators_ != "")
00393 filename = g_options.actuators_;
00394 TiXmlDocument xml(filename);
00395
00396 if (xml.LoadFile())
00397 {
00398 parseConfig(xml.RootElement());
00399 }
00400 else
00401 {
00402 Usage("Unable to load configuration file");
00403 }
00404
00405 if (g_options.help_)
00406 Usage();
00407
00408 if (optind < argc)
00409 {
00410 Usage("Extra arguments");
00411 }
00412
00413 if (!g_options.interface_)
00414 Usage("You must specify a network interface");
00415
00416
00417 int test_sock = socket(PF_PACKET, SOCK_RAW, htons(0x88A4));
00418 if ((test_sock < 0) && (errno == EPERM))
00419 {
00420 ROS_FATAL("Insufficient priviledges to obtain raw socket. Try running as root.");
00421 exit(-1);
00422 }
00423 close(test_sock);
00424
00425
00426 if (mlockall(MCL_CURRENT | MCL_FUTURE) != 0)
00427 {
00428 ROS_WARN("mlockall failed : %s", strerror(errno));
00429 }
00430
00431 init(g_options.interface_);
00432
00433 if (g_options.program_)
00434 {
00435 string board = "wg005";
00436 if (!g_options.name_)
00437 Usage("You must specify a name");
00438 if (g_options.motor_ == "")
00439 {
00440 if (actuators.find(g_options.name_) == actuators.end())
00441 Usage("No default motor for this name");
00442 g_options.motor_ = actuators[g_options.name_].motor;
00443 board = actuators[g_options.name_].board;
00444 }
00445 if (g_options.board_ != "") {
00446 board = g_options.board_;
00447 }
00448 if (g_options.device_ == -1)
00449 Usage("You must specify a device #");
00450 if (motors.find(g_options.motor_) == motors.end())
00451 Usage("You must specify a valid motor");
00452
00453 programDevice(g_options.device_, motors[g_options.motor_], g_options.name_, board);
00454 }
00455
00456 return 0;
00457 }