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 "ethercat_hardware/ethercat_hardware.h"
00036
00037 #include <ethercat/ethercat_xenomai_drv.h>
00038 #include <dll/ethercat_dll.h>
00039 #include <dll/ethercat_device_addressed_telegram.h>
00040
00041 #include <sstream>
00042
00043 #include <net/if.h>
00044 #include <sys/ioctl.h>
00045 #include <boost/foreach.hpp>
00046 #include <boost/regex.hpp>
00047
00048 EthercatHardwareDiagnostics::EthercatHardwareDiagnostics() :
00049
00050 txandrx_errors_(0),
00051 device_count_(0),
00052 pd_error_(false),
00053 halt_after_reset_(false),
00054 reset_motors_service_count_(0),
00055 halt_motors_service_count_(0),
00056 halt_motors_error_count_(0),
00057 motors_halted_(false),
00058 motors_halted_reason_("")
00059 {
00060 resetMaxTiming();
00061 }
00062
00063 void EthercatHardwareDiagnostics::resetMaxTiming()
00064 {
00065 max_pack_command_ = 0.0;
00066 max_txandrx_ = 0.0;
00067 max_unpack_state_ = 0.0;
00068 max_publish_ = 0.0;
00069 }
00070
00071 EthercatHardware::EthercatHardware(const std::string& name) :
00072 hw_(0), node_(ros::NodeHandle(name)),
00073 ni_(0), this_buffer_(0), prev_buffer_(0), buffer_size_(0), halt_motors_(true), reset_state_(0),
00074 max_pd_retries_(10),
00075 diagnostics_publisher_(node_),
00076 motor_publisher_(node_, "motors_halted", 1, true),
00077 device_loader_("ethercat_hardware", "EthercatDevice")
00078 {
00079
00080 }
00081
00082 EthercatHardware::~EthercatHardware()
00083 {
00084 diagnostics_publisher_.stop();
00085 for (uint32_t i = 0; i < slaves_.size(); ++i)
00086 {
00087 EC_FixedStationAddress fsa(i + 1);
00088 EtherCAT_SlaveHandler *sh = em_->get_slave_handler(fsa);
00089 if (sh) sh->to_state(EC_PREOP_STATE);
00090 }
00091 if (ni_)
00092 {
00093 close_socket(ni_);
00094 }
00095 delete[] buffers_;
00096 delete hw_;
00097 delete oob_com_;
00098 motor_publisher_.stop();
00099 }
00100
00101
00102 void EthercatHardware::changeState(EtherCAT_SlaveHandler *sh, EC_State new_state)
00103 {
00104 unsigned product_code = sh->get_product_code();
00105 unsigned serial = sh->get_serial();
00106 uint32_t revision = sh->get_revision();
00107 unsigned slave = sh->get_station_address()-1;
00108
00109 if (!sh->to_state(new_state))
00110 {
00111 ROS_FATAL("Cannot goto state %d for slave #%d, product code: %u (0x%X), serial: %u (0x%X), revision: %d (0x%X)",
00112 new_state, slave, product_code, product_code, serial, serial, revision, revision);
00113 if ((product_code==0xbaddbadd) || (serial==0xbaddbadd) || (revision==0xbaddbadd))
00114 ROS_FATAL("Note: 0xBADDBADD indicates that the value was not read correctly from device.");
00115 exit(EXIT_FAILURE);
00116 }
00117 }
00118
00119 void EthercatHardware::init(char *interface, bool allow_unprogrammed)
00120 {
00121
00122 int sock = socket(PF_INET, SOCK_DGRAM, 0);
00123 if (sock < 0) {
00124 int error = errno;
00125 ROS_FATAL("Couldn't open temp socket : %s", strerror(error));
00126 sleep(1);
00127 exit(EXIT_FAILURE);
00128 }
00129
00130 struct ifreq ifr;
00131 strncpy(ifr.ifr_name, interface, IFNAMSIZ);
00132 if (ioctl(sock, SIOCGIFFLAGS, &ifr) < 0) {
00133 int error = errno;
00134 ROS_FATAL("Cannot get interface flags for %s: %s", interface, strerror(error));
00135 sleep(1);
00136 exit(EXIT_FAILURE);
00137 }
00138
00139 close(sock);
00140 sock = -1;
00141
00142 if (!(ifr.ifr_flags & IFF_UP)) {
00143 ROS_FATAL("Interface %s is not UP. Try : ifup %s", interface, interface);
00144 sleep(1);
00145 exit(EXIT_FAILURE);
00146 }
00147 if (!(ifr.ifr_flags & IFF_RUNNING)) {
00148 ROS_FATAL("Interface %s is not RUNNING. Is cable plugged in and device powered?", interface);
00149 sleep(1);
00150 exit(EXIT_FAILURE);
00151 }
00152
00153
00154
00155 interface_ = interface;
00156 if ((ni_ = init_ec(interface)) == NULL)
00157 {
00158 ROS_FATAL("Unable to initialize interface: %s", interface);
00159 sleep(1);
00160 exit(EXIT_FAILURE);
00161 }
00162
00163 oob_com_ = new EthercatOobCom(ni_);
00164
00165
00166 EtherCAT_DataLinkLayer::instance()->attach(ni_);
00167 if ((al_ = EtherCAT_AL::instance()) == NULL)
00168 {
00169 ROS_FATAL("Unable to initialize Application Layer (AL): %p", al_);
00170 sleep(1);
00171 exit(EXIT_FAILURE);
00172 }
00173
00174 int num_ethercat_devices_ = al_->get_num_slaves();
00175 if (num_ethercat_devices_ == 0)
00176 {
00177 ROS_FATAL("Unable to locate any slaves");
00178 sleep(1);
00179 exit(EXIT_FAILURE);
00180 }
00181
00182
00183 if ((em_ = EtherCAT_Master::instance()) == NULL)
00184 {
00185 ROS_FATAL("Unable to initialize EtherCAT_Master: %p", em_);
00186 sleep(1);
00187 exit(EXIT_FAILURE);
00188 }
00189
00190
00191 slaves_.resize(num_ethercat_devices_);
00192
00193
00194 std::vector<EtherCAT_SlaveHandler*> slave_handles;
00195 for (unsigned int slave = 0; slave < slaves_.size(); ++slave)
00196 {
00197 EC_FixedStationAddress fsa(slave + 1);
00198 EtherCAT_SlaveHandler *sh = em_->get_slave_handler(fsa);
00199 if (sh == NULL)
00200 {
00201 ROS_FATAL("Unable to get slave handler #%d", slave);
00202 sleep(1);
00203 exit(EXIT_FAILURE);
00204 }
00205 slave_handles.push_back(sh);
00206 }
00207
00208
00209 BOOST_FOREACH(EtherCAT_SlaveHandler *sh, slave_handles)
00210 {
00211 unsigned slave = sh->get_station_address()-1;
00212 if ((slaves_[slave] = configSlave(sh)) == NULL)
00213 {
00214 ROS_FATAL("Unable to configure slave #%d", slave);
00215 sleep(1);
00216 exit(EXIT_FAILURE);
00217 }
00218 buffer_size_ += slaves_[slave]->command_size_ + slaves_[slave]->status_size_;
00219 }
00220
00221
00222 loadNonEthercatDevices();
00223
00224
00225 BOOST_FOREACH(EtherCAT_SlaveHandler *sh, slave_handles)
00226 {
00227 changeState(sh,EC_PREOP_STATE);
00228 }
00229
00230
00231 BOOST_FOREACH(EtherCAT_SlaveHandler *sh, slave_handles)
00232 {
00233 changeState(sh,EC_SAFEOP_STATE);
00234 }
00235
00236
00237
00238 BOOST_FOREACH(EtherCAT_SlaveHandler *sh, slave_handles)
00239 {
00240 changeState(sh,EC_OP_STATE);
00241 }
00242
00243
00244 buffers_ = new unsigned char[2 * buffer_size_];
00245 this_buffer_ = buffers_;
00246 prev_buffer_ = buffers_ + buffer_size_;
00247
00248
00249 memset(this_buffer_, 0, 2 * buffer_size_);
00250 if (!txandrx_PD(buffer_size_, this_buffer_, 20))
00251 {
00252 ROS_FATAL("No communication with devices");
00253 sleep(1);
00254 exit(EXIT_FAILURE);
00255 }
00256
00257
00258 memcpy(prev_buffer_, this_buffer_, buffer_size_);
00259
00260
00261 hw_ = new pr2_hardware_interface::HardwareInterface();
00262 hw_->current_time_ = ros::Time::now();
00263 last_published_ = hw_->current_time_;
00264
00265
00266
00267 for (unsigned int slave = 0; slave < slaves_.size(); ++slave)
00268 {
00269 if (slaves_[slave]->initialize(hw_, allow_unprogrammed) < 0)
00270 {
00271 EtherCAT_SlaveHandler *sh = slaves_[slave]->sh_;
00272 if (sh != NULL)
00273 {
00274 ROS_FATAL("Unable to initialize slave #%d, product code: %d, revision: %d, serial: %d",
00275 slave, sh->get_product_code(), sh->get_revision(), sh->get_serial());
00276 sleep(1);
00277 }
00278 else
00279 {
00280 ROS_FATAL("Unable to initialize slave #%d", slave);
00281 }
00282 exit(EXIT_FAILURE);
00283 }
00284 }
00285
00286
00287 {
00288
00289
00290 static const int MAX_TIMEOUT = 100000;
00291 static const int DEFAULT_TIMEOUT = 20000;
00292 int timeout;
00293 if (!node_.getParam("realtime_socket_timeout", timeout))
00294 {
00295 timeout = DEFAULT_TIMEOUT;
00296 }
00297 if ((timeout <= 1) || (timeout > MAX_TIMEOUT))
00298 {
00299 int old_timeout = timeout;
00300 timeout = std::max(1, std::min(MAX_TIMEOUT, timeout));
00301 ROS_WARN("Invalid timeout (%d) for socket, using %d", old_timeout, timeout);
00302 }
00303 if (set_socket_timeout(ni_, timeout))
00304 {
00305 ROS_FATAL("Error setting socket timeout to %d", timeout);
00306 sleep(1);
00307 exit(EXIT_FAILURE);
00308 }
00309 timeout_ = timeout;
00310
00311
00312
00313
00314
00315
00316
00317
00318
00319
00320 int max_pd_retries = MAX_TIMEOUT / timeout;
00321 static const int MAX_RETRIES=50, MIN_RETRIES=1;
00322 node_.getParam("max_pd_retries", max_pd_retries);
00323
00324 if ((max_pd_retries * timeout) > (MAX_TIMEOUT))
00325 {
00326 max_pd_retries = MAX_TIMEOUT / timeout;
00327 ROS_WARN("Max PD retries is too large for given timeout. Limiting value to %d", max_pd_retries);
00328 }
00329 if ((max_pd_retries < MIN_RETRIES) || (max_pd_retries > MAX_RETRIES))
00330 {
00331 max_pd_retries = std::max(MIN_RETRIES,std::min(MAX_RETRIES,max_pd_retries));
00332 ROS_WARN("Limiting max PD retries to %d", max_pd_retries);
00333 }
00334 max_pd_retries = std::max(MIN_RETRIES,std::min(MAX_RETRIES,max_pd_retries));
00335 max_pd_retries_ = max_pd_retries;
00336 }
00337
00338 diagnostics_publisher_.initialize(interface_, buffer_size_, slaves_, num_ethercat_devices_, timeout_, max_pd_retries_);
00339 }
00340
00341
00342 EthercatHardwareDiagnosticsPublisher::EthercatHardwareDiagnosticsPublisher(ros::NodeHandle &node) :
00343 node_(node),
00344 diagnostics_ready_(false),
00345 publisher_(node_.advertise<diagnostic_msgs::DiagnosticArray>("/diagnostics", 1)),
00346 diagnostics_buffer_(NULL),
00347 last_dropped_packet_count_(0),
00348 last_dropped_packet_time_(0)
00349 {
00350 }
00351
00352 EthercatHardwareDiagnosticsPublisher::~EthercatHardwareDiagnosticsPublisher()
00353 {
00354 delete[] diagnostics_buffer_;
00355 }
00356
00357 void EthercatHardwareDiagnosticsPublisher::initialize(const string &interface, unsigned int buffer_size,
00358 const std::vector<boost::shared_ptr<EthercatDevice> > &slaves,
00359 unsigned int num_ethercat_devices,
00360 unsigned timeout, unsigned max_pd_retries)
00361 {
00362 interface_ = interface;
00363 buffer_size_ = buffer_size;
00364 slaves_ = slaves;
00365 num_ethercat_devices_ = num_ethercat_devices;
00366 timeout_ = timeout;
00367 max_pd_retries_ = max_pd_retries;
00368
00369 diagnostics_buffer_ = new unsigned char[buffer_size_];
00370
00371
00372 diagnostic_array_.status.reserve(slaves_.size() + 1);
00373 values_.reserve(10);
00374
00375 ethernet_interface_info_.initialize(interface);
00376
00377 diagnostics_thread_ = boost::thread(boost::bind(&EthercatHardwareDiagnosticsPublisher::diagnosticsThreadFunc, this));
00378 }
00379
00380 void EthercatHardwareDiagnosticsPublisher::publish(
00381 const unsigned char *buffer,
00382 const EthercatHardwareDiagnostics &diagnostics)
00383 {
00384 boost::try_to_lock_t try_lock;
00385 boost::unique_lock<boost::mutex> lock(diagnostics_mutex_, try_lock);
00386 if (lock.owns_lock())
00387 {
00388
00389 memcpy(diagnostics_buffer_, buffer, buffer_size_);
00390 diagnostics_ = diagnostics;
00391
00392 diagnostics_ready_ = true;
00393 diagnostics_cond_.notify_one();
00394 }
00395 }
00396
00397 void EthercatHardwareDiagnosticsPublisher::stop()
00398 {
00399 diagnostics_thread_.interrupt();
00400 diagnostics_thread_.join();
00401 publisher_.shutdown();
00402 }
00403
00404 void EthercatHardwareDiagnosticsPublisher::diagnosticsThreadFunc()
00405 {
00406 try {
00407 while (1) {
00408 boost::unique_lock<boost::mutex> lock(diagnostics_mutex_);
00409 while (!diagnostics_ready_) {
00410 diagnostics_cond_.wait(lock);
00411 }
00412 diagnostics_ready_ = false;
00413 publishDiagnostics();
00414 }
00415 } catch (boost::thread_interrupted const&) {
00416 return;
00417 }
00418 }
00419
00420 void EthercatHardwareDiagnosticsPublisher::timingInformation(
00421 diagnostic_updater::DiagnosticStatusWrapper &status,
00422 const string &key,
00423 const accumulator_set<double, stats<tag::max, tag::mean> > &acc,
00424 double max)
00425 {
00426 status.addf(key + " Avg (us)", "%5.4f", extract_result<tag::mean>(acc) * 1e6);
00427 status.addf(key + " 1 Sec Max (us)", "%5.4f", extract_result<tag::max>(acc) * 1e6);
00428 status.addf(key + " Max (us)", "%5.4f", max * 1e6);
00429 }
00430
00431 void EthercatHardwareDiagnosticsPublisher::publishDiagnostics()
00432 {
00433 ros::Time now(ros::Time::now());
00434
00435
00436 status_.clearSummary();
00437 status_.clear();
00438
00439 status_.name = "EtherCAT Master";
00440 if (diagnostics_.motors_halted_)
00441 {
00442 std::ostringstream desc;
00443 desc << "Motors halted";
00444 if (diagnostics_.halt_after_reset_)
00445 {
00446 desc << " soon after reset";
00447 }
00448 desc << " (" << diagnostics_.motors_halted_reason_ << ")";
00449 status_.summary(status_.ERROR, desc.str());
00450 } else {
00451 status_.summary(status_.OK, "OK");
00452 }
00453
00454 if (diagnostics_.pd_error_)
00455 {
00456 status_.mergeSummary(status_.ERROR, "Error sending proccess data");
00457 }
00458
00459 status_.add("Motors halted", diagnostics_.motors_halted_ ? "true" : "false");
00460 status_.addf("EtherCAT devices (expected)", "%d", num_ethercat_devices_);
00461 status_.addf("EtherCAT devices (current)", "%d", diagnostics_.device_count_);
00462 ethernet_interface_info_.publishDiagnostics(status_);
00463
00464
00465 status_.addf("Timeout (us)", "%d", timeout_);
00466 status_.addf("Max PD Retries", "%d", max_pd_retries_);
00467
00468
00469 if (num_ethercat_devices_ != diagnostics_.device_count_) {
00470 status_.mergeSummary(status_.WARN, "Number of EtherCAT devices changed");
00471 }
00472
00473 timingInformation(status_, "Roundtrip time", diagnostics_.txandrx_acc_, diagnostics_.max_txandrx_);
00474 if (diagnostics_.collect_extra_timing_)
00475 {
00476 timingInformation(status_, "Pack command time", diagnostics_.pack_command_acc_, diagnostics_.max_pack_command_);
00477 timingInformation(status_, "Unpack state time", diagnostics_.unpack_state_acc_, diagnostics_.max_unpack_state_);
00478 timingInformation(status_, "Publish time", diagnostics_.publish_acc_, diagnostics_.max_publish_);
00479 }
00480
00481 status_.addf("EtherCAT Process Data txandrx errors", "%d", diagnostics_.txandrx_errors_);
00482
00483 status_.addf("Reset motors service count", "%d", diagnostics_.reset_motors_service_count_);
00484 status_.addf("Halt motors service count", "%d", diagnostics_.halt_motors_service_count_);
00485 status_.addf("Halt motors error count", "%d", diagnostics_.halt_motors_error_count_);
00486
00487 {
00488 const struct netif_counters *c = &diagnostics_.counters_;
00489 status_.add("Input Thread", (diagnostics_.input_thread_is_stopped_ ? "Stopped" : "Running"));
00490 status_.addf("Sent Packets", "%llu", (unsigned long long)c->sent);
00491 status_.addf("Received Packets", "%llu", (unsigned long long)c->received);
00492 status_.addf("Collected Packets", "%llu", (unsigned long long)c->collected);
00493 status_.addf("Dropped Packets", "%llu", (unsigned long long)c->dropped);
00494 status_.addf("TX Errors", "%llu", (unsigned long long)c->tx_error);
00495 status_.addf("TX Network Down", "%llu", (unsigned long long)c->tx_net_down);
00496 status_.addf("TX Would Block", "%llu", (unsigned long long)c->tx_would_block);
00497 status_.addf("TX No Buffers", "%llu", (unsigned long long)c->tx_no_bufs);
00498 status_.addf("TX Queue Full", "%llu", (unsigned long long)c->tx_full);
00499 status_.addf("RX Runt Packet", "%llu", (unsigned long long)c->rx_runt_pkt);
00500 status_.addf("RX Not EtherCAT", "%llu", (unsigned long long)c->rx_not_ecat);
00501 status_.addf("RX Other EML", "%llu", (unsigned long long)c->rx_other_eml);
00502 status_.addf("RX Bad Index", "%llu", (unsigned long long)c->rx_bad_index);
00503 status_.addf("RX Bad Sequence", "%llu", (unsigned long long)c->rx_bad_seqnum);
00504 status_.addf("RX Duplicate Sequence", "%llu", (unsigned long long)c->rx_dup_seqnum);
00505 status_.addf("RX Duplicate Packet", "%llu", (unsigned long long)c->rx_dup_pkt);
00506 status_.addf("RX Bad Order", "%llu", (unsigned long long)c->rx_bad_order);
00507 status_.addf("RX Late Packet", "%llu", (unsigned long long)c->rx_late_pkt);
00508 status_.addf("RX Late Packet RTT", "%llu", (unsigned long long)c->rx_late_pkt_rtt_us);
00509
00510 double rx_late_pkt_rtt_us_avg = 0.0;
00511 if (c->rx_late_pkt > 0) {
00512 rx_late_pkt_rtt_us_avg = ((double)c->rx_late_pkt_rtt_us_sum)/((double)c->rx_late_pkt);
00513 }
00514 status_.addf("RX Late Packet Avg RTT", "%f", rx_late_pkt_rtt_us_avg);
00515
00516
00517 if (c->dropped > last_dropped_packet_count_)
00518 {
00519 last_dropped_packet_time_ = now;
00520 last_dropped_packet_count_ = c->dropped;
00521 }
00522 }
00523
00524
00525 if ((last_dropped_packet_count_ > 0) && ((now - last_dropped_packet_time_).toSec() < dropped_packet_warning_hold_time_))
00526 {
00527 status_.mergeSummaryf(status_.WARN, "Dropped packets in last %d seconds", dropped_packet_warning_hold_time_);
00528 }
00529
00530 diagnostic_array_.status.clear();
00531 diagnostic_array_.status.push_back(status_);
00532
00533
00534 unsigned char *current = diagnostics_buffer_;
00535 for (unsigned int s = 0; s < slaves_.size(); ++s)
00536 {
00537 slaves_[s]->multiDiagnostics(diagnostic_array_.status, current);
00538 current += slaves_[s]->command_size_ + slaves_[s]->status_size_;
00539 }
00540
00541
00542 diagnostic_array_.header.stamp = ros::Time::now();
00543 publisher_.publish(diagnostic_array_);
00544 }
00545
00546 void EthercatHardware::update(bool reset, bool halt)
00547 {
00548
00549 ros::Time update_start_time(ros::Time::now());
00550
00551 unsigned char *this_buffer, *prev_buffer;
00552
00553
00554 this_buffer = this_buffer_;
00555
00556 if (halt)
00557 {
00558 ++diagnostics_.halt_motors_service_count_;
00559 haltMotors(false , "service request");
00560 }
00561
00562
00563
00564 const unsigned CYCLES_PER_HALT_RELEASE = 2;
00565 if (reset)
00566 {
00567 ++diagnostics_.reset_motors_service_count_;
00568 reset_state_ = CYCLES_PER_HALT_RELEASE * slaves_.size() + 5;
00569 last_reset_ = update_start_time;
00570 diagnostics_.halt_after_reset_ = false;
00571 }
00572 bool reset_devices = reset_state_ == CYCLES_PER_HALT_RELEASE * slaves_.size() + 3;
00573 if (reset_devices)
00574 {
00575 halt_motors_ = false;
00576 diagnostics_.motors_halted_ = false;
00577 diagnostics_.motors_halted_reason_ = "";
00578 diagnostics_.resetMaxTiming();
00579 diagnostics_.pd_error_ = false;
00580 }
00581
00582 for (unsigned int s = 0; s < slaves_.size(); ++s)
00583 {
00584
00585
00586 bool halt_device = halt_motors_ || ((s*CYCLES_PER_HALT_RELEASE+1) < reset_state_);
00587 slaves_[s]->packCommand(this_buffer, halt_device, reset_devices);
00588 this_buffer += slaves_[s]->command_size_ + slaves_[s]->status_size_;
00589 }
00590
00591
00592 ros::Time txandrx_start_time(ros::Time::now());
00593 diagnostics_.pack_command_acc_((txandrx_start_time-update_start_time).toSec());
00594
00595
00596 bool success = txandrx_PD(buffer_size_, this_buffer_, max_pd_retries_);
00597
00598 ros::Time txandrx_end_time(ros::Time::now());
00599 diagnostics_.txandrx_acc_((txandrx_end_time - txandrx_start_time).toSec());
00600
00601 hw_->current_time_ = txandrx_end_time;
00602
00603 if (!success)
00604 {
00605
00606 haltMotors(true , "communication error");
00607 diagnostics_.pd_error_ = true;
00608 }
00609 else
00610 {
00611
00612 this_buffer = this_buffer_;
00613 prev_buffer = prev_buffer_;
00614 for (unsigned int s = 0; s < slaves_.size(); ++s)
00615 {
00616 if (!slaves_[s]->unpackState(this_buffer, prev_buffer) && !reset_devices)
00617 {
00618 haltMotors(true , "device error");
00619 }
00620 this_buffer += slaves_[s]->command_size_ + slaves_[s]->status_size_;
00621 prev_buffer += slaves_[s]->command_size_ + slaves_[s]->status_size_;
00622 }
00623
00624 if (reset_state_)
00625 --reset_state_;
00626
00627 unsigned char *tmp = this_buffer_;
00628 this_buffer_ = prev_buffer_;
00629 prev_buffer_ = tmp;
00630 }
00631
00632 ros::Time unpack_end_time;
00633 if (diagnostics_.collect_extra_timing_)
00634 {
00635 unpack_end_time = ros::Time::now();
00636 diagnostics_.unpack_state_acc_((unpack_end_time - txandrx_end_time).toSec());
00637 }
00638
00639 if ((update_start_time - last_published_) > ros::Duration(1.0))
00640 {
00641 last_published_ = update_start_time;
00642 publishDiagnostics();
00643 motor_publisher_.lock();
00644 motor_publisher_.msg_.data = halt_motors_;
00645 motor_publisher_.unlockAndPublish();
00646 }
00647
00648 if (diagnostics_.collect_extra_timing_)
00649 {
00650 ros::Time publish_end_time(ros::Time::now());
00651 diagnostics_.publish_acc_((publish_end_time - unpack_end_time).toSec());
00652 }
00653 }
00654
00655
00656 void EthercatHardware::haltMotors(bool error, const char* reason)
00657 {
00658 if (!halt_motors_)
00659 {
00660
00661 motor_publisher_.lock();
00662 motor_publisher_.msg_.data = halt_motors_;
00663 motor_publisher_.unlockAndPublish();
00664
00665 diagnostics_.motors_halted_reason_=reason;
00666 if (error)
00667 {
00668 ++diagnostics_.halt_motors_error_count_;
00669 if ((ros::Time::now() - last_reset_) < ros::Duration(0.5))
00670 {
00671
00672 diagnostics_.halt_after_reset_ = true;
00673 }
00674 }
00675 }
00676
00677 diagnostics_.motors_halted_ = true;
00678 halt_motors_ = true;
00679 }
00680
00681
00682 void EthercatHardware::updateAccMax(double &max, const accumulator_set<double, stats<tag::max, tag::mean> > &acc)
00683 {
00684 max = std::max(max, extract_result<tag::max>(acc));
00685 }
00686
00687 void EthercatHardware::publishDiagnostics()
00688 {
00689
00690 updateAccMax(diagnostics_.max_pack_command_, diagnostics_.pack_command_acc_);
00691 updateAccMax(diagnostics_.max_txandrx_, diagnostics_.txandrx_acc_);
00692 updateAccMax(diagnostics_.max_unpack_state_, diagnostics_.unpack_state_acc_);
00693 updateAccMax(diagnostics_.max_publish_, diagnostics_.publish_acc_);
00694
00695
00696 diagnostics_.counters_ = ni_->counters;
00697 diagnostics_.input_thread_is_stopped_ = bool(ni_->is_stopped);
00698
00699 diagnostics_.motors_halted_ = halt_motors_;
00700
00701
00702 diagnostics_publisher_.publish(this_buffer_, diagnostics_);
00703
00704
00705 static accumulator_set<double, stats<tag::max, tag::mean> > blank;
00706 diagnostics_.pack_command_acc_ = blank;
00707 diagnostics_.txandrx_acc_ = blank;
00708 diagnostics_.unpack_state_acc_ = blank;
00709 diagnostics_.publish_acc_ = blank;
00710 }
00711
00712
00713 boost::shared_ptr<EthercatDevice>
00714 EthercatHardware::configSlave(EtherCAT_SlaveHandler *sh)
00715 {
00716 static int start_address = 0x00010000;
00717 boost::shared_ptr<EthercatDevice> p;
00718 unsigned product_code = sh->get_product_code();
00719 unsigned serial = sh->get_serial();
00720 uint32_t revision = sh->get_revision();
00721 unsigned slave = sh->get_station_address()-1;
00722
00723
00724
00725
00726
00727
00728
00729
00730
00731
00732
00733
00734
00735
00736
00737
00738
00739
00740
00741
00742
00743
00744
00745
00746
00747
00748
00749
00750
00751
00752
00753 stringstream class_name_regex_str;
00754 class_name_regex_str << "(.*/)?" << product_code;
00755 boost::regex class_name_regex(class_name_regex_str.str(), boost::regex::extended);
00756
00757 std::vector<std::string> classes = device_loader_.getDeclaredClasses();
00758 std::string matching_class_name;
00759
00760 BOOST_FOREACH(const std::string &class_name, classes)
00761 {
00762 if (regex_match(class_name, class_name_regex))
00763 {
00764 if (matching_class_name.size() != 0)
00765 {
00766 ROS_ERROR("Found more than 1 EtherCAT driver for device with product code : %d", product_code);
00767 ROS_ERROR("First class name = '%s'. Second class name = '%s'",
00768 class_name.c_str(), matching_class_name.c_str());
00769 }
00770 matching_class_name = class_name;
00771 }
00772 }
00773
00774 if (matching_class_name.size() != 0)
00775 {
00776
00777
00778 try {
00779 p = device_loader_.createInstance(matching_class_name);
00780 }
00781 catch (pluginlib::LibraryLoadException &e)
00782 {
00783 p.reset();
00784 ROS_FATAL("Unable to load plugin for slave #%d, product code: %u (0x%X), serial: %u (0x%X), revision: %d (0x%X)",
00785 slave, product_code, product_code, serial, serial, revision, revision);
00786 ROS_FATAL("%s", e.what());
00787 }
00788 }
00789 else
00790 {
00791 if ((product_code==0xbaddbadd) || (serial==0xbaddbadd) || (revision==0xbaddbadd))
00792 {
00793 ROS_FATAL("Note: 0xBADDBADD indicates that the value was not read correctly from device.");
00794 ROS_FATAL("Perhaps you should power-cycle the MCBs");
00795 }
00796 else
00797 {
00798 ROS_ERROR("Unable to load plugin for slave #%d, product code: %u (0x%X), serial: %u (0x%X), revision: %d (0x%X)",
00799 slave, product_code, product_code, serial, serial, revision, revision);
00800 ROS_ERROR("Possible classes:");
00801 BOOST_FOREACH(const std::string &class_name, classes)
00802 {
00803 ROS_ERROR(" %s", class_name.c_str());
00804 }
00805
00806
00807
00808 }
00809 }
00810
00811 if (p != NULL)
00812 {
00813 p->construct(sh, start_address);
00814 }
00815
00816 return p;
00817 }
00818
00819
00820 boost::shared_ptr<EthercatDevice>
00821 EthercatHardware::configNonEthercatDevice(const std::string &name, const std::string &type)
00822 {
00823 boost::shared_ptr<EthercatDevice> p;
00824 try {
00825 p = device_loader_.createInstance(type);
00826 }
00827 catch (pluginlib::LibraryLoadException &e)
00828 {
00829 p.reset();
00830 ROS_FATAL("Unable to load plugin for non-EtherCAT device '%s' with type: %s : %s"
00831 , name.c_str(), type.c_str(), e.what());
00832 }
00833 if (p) {
00834 ROS_INFO("Creating non-EtherCAT device '%s' of type '%s'", name.c_str(), type.c_str());
00835 ros::NodeHandle nh(node_, "non_ethercat_devices/"+name);
00836 p->construct(nh);
00837 }
00838 return p;
00839 }
00840
00841
00842
00843 class MyXmlRpcValue : public XmlRpc::XmlRpcValue
00844 {
00845 public:
00846 MyXmlRpcValue(XmlRpc::XmlRpcValue &value) : XmlRpc::XmlRpcValue(value) { }
00847 XmlRpcValue::ValueStruct &getMap() {return *_value.asStruct;}
00848 };
00849
00850 void EthercatHardware::loadNonEthercatDevices()
00851 {
00852
00853
00854
00855
00856
00857
00858
00859
00860
00861
00862
00863
00864
00865
00866
00867
00868
00869
00870
00871
00872
00873 if (!node_.hasParam("non_ethercat_devices"))
00874 {
00875
00876 return;
00877 }
00878
00879 XmlRpc::XmlRpcValue devices;
00880 node_.getParam("non_ethercat_devices", devices);
00881 if (devices.getType() != XmlRpc::XmlRpcValue::TypeStruct)
00882 {
00883 ROS_ERROR("Rosparam 'non_ethercat_devices' is not a struct type");
00884 return;
00885 }
00886
00887 MyXmlRpcValue my_devices(devices);
00888 typedef XmlRpc::XmlRpcValue::ValueStruct::value_type map_item_t ;
00889 BOOST_FOREACH(map_item_t &item, my_devices.getMap())
00890 {
00891 const std::string &name(item.first);
00892 XmlRpc::XmlRpcValue &device_info(item.second);
00893
00894 if (device_info.getType() != XmlRpc::XmlRpcValue::TypeStruct)
00895 {
00896 ROS_ERROR("non_ethercat_devices/%s is not a struct type", name.c_str());
00897 continue;
00898 }
00899
00900 if (!device_info.hasMember("type"))
00901 {
00902 ROS_ERROR("non_ethercat_devices/%s 'type' element", name.c_str());
00903 continue;
00904 }
00905
00906 std::string type(static_cast<std::string>(device_info["type"]));
00907
00908 boost::shared_ptr<EthercatDevice> new_device =
00909 configNonEthercatDevice(name,type);
00910 if (new_device != NULL)
00911 {
00912 slaves_.push_back(new_device);
00913 }
00914 }
00915 }
00916
00917
00918 void EthercatHardware::collectDiagnostics()
00919 {
00920 if (NULL == oob_com_)
00921 return;
00922
00923 {
00924 EC_Logic *logic = EC_Logic::instance();
00925 unsigned char p[1];
00926 EC_UINT length = sizeof(p);
00927
00928
00929 APRD_Telegram status(logic->get_idx(),
00930 0,
00931 0,
00932 logic->get_wkc(),
00933 length,
00934 p);
00935
00936
00937 EC_Ethernet_Frame frame(&status);
00938 oob_com_->txandrx(&frame);
00939
00940
00941 diagnostics_.device_count_ = status.get_adp();
00942 }
00943
00944 for (unsigned i = 0; i < slaves_.size(); ++i)
00945 {
00946 boost::shared_ptr<EthercatDevice> d(slaves_[i]);
00947 d->collectDiagnostics(oob_com_);
00948 }
00949 }
00950
00951
00952
00953 void EthercatHardware::printCounters(std::ostream &os)
00954 {
00955 const struct netif_counters &c(ni_->counters);
00956 os << "netif counters :" << endl
00957 << " sent = " << c.sent << endl
00958 << " received = " << c.received << endl
00959 << " collected = " << c.collected << endl
00960 << " dropped = " << c.dropped << endl
00961 << " tx_error = " << c.tx_error << endl
00962 << " tx_net_down = " << c.tx_net_down << endl
00963 << " tx_would_block= " << c.tx_would_block << endl
00964 << " tx_no_bufs = " << c.tx_no_bufs << endl
00965 << " tx_full = " << c.tx_full << endl
00966 << " rx_runt_pkt = " << c.rx_runt_pkt << endl
00967 << " rx_not_ecat = " << c.rx_not_ecat << endl
00968 << " rx_other_eml = " << c.rx_other_eml << endl
00969 << " rx_bad_index = " << c.rx_bad_index << endl
00970 << " rx_bad_seqnum = " << c.rx_bad_seqnum << endl
00971 << " rx_dup_seqnum = " << c.rx_dup_seqnum << endl
00972 << " rx_dup_pkt = " << c.rx_dup_pkt << endl
00973 << " rx_bad_order = " << c.rx_bad_order << endl
00974 << " rx_late_pkt = " << c.rx_late_pkt << endl;
00975 }
00976
00977
00978 bool EthercatHardware::txandrx_PD(unsigned buffer_size, unsigned char* buffer, unsigned tries)
00979 {
00980
00981 bool success = false;
00982 for (unsigned i=0; i<tries && !success; ++i) {
00983
00984 success = em_->txandrx_PD(buffer_size_, this_buffer_);
00985 if (!success) {
00986 ++diagnostics_.txandrx_errors_;
00987 }
00988
00989 oob_com_->tx();
00990 }
00991 return success;
00992 }
00993
00994
00995 bool EthercatHardware::publishTrace(int position, const string &reason, unsigned level, unsigned delay)
00996 {
00997 if (position >= (int)slaves_.size())
00998 {
00999 ROS_WARN("Invalid device position %d. Use 0-%d, or -1.", position, int(slaves_.size())-1);
01000 return false;
01001 }
01002
01003 if (level > 2)
01004 {
01005 ROS_WARN("Invalid level : %d. Using level=2 (ERROR).", level);
01006 level = 2;
01007 }
01008
01009 string new_reason("Manually triggered : " + reason);
01010
01011 bool retval = false;
01012 if (position < 0)
01013 {
01014 for (unsigned i=0; i<slaves_.size(); ++i)
01015 {
01016 if (slaves_[i]->publishTrace(new_reason,level,delay))
01017 {
01018 retval = true;
01019 }
01020 }
01021 }
01022 else
01023 {
01024 retval = slaves_[position]->publishTrace(new_reason,level,delay);
01025 if (!retval)
01026 {
01027 ROS_WARN("Device %d does not support publishing trace", position);
01028 }
01029 }
01030 return retval;
01031 }