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 <net/if.h>
00042 #include <sys/ioctl.h>
00043 #include <boost/foreach.hpp>
00044
00045 EthercatHardwareDiagnostics::EthercatHardwareDiagnostics() :
00046
00047 txandrx_errors_(0),
00048 device_count_(0),
00049 pd_error_(false),
00050 halt_after_reset_(false),
00051 reset_motors_service_count_(0),
00052 halt_motors_service_count_(0),
00053 halt_motors_error_count_(0)
00054 {
00055 resetMaxTiming();
00056 }
00057
00058 void EthercatHardwareDiagnostics::resetMaxTiming()
00059 {
00060 max_pack_command_ = 0.0;
00061 max_txandrx_ = 0.0;
00062 max_unpack_state_ = 0.0;
00063 max_publish_ = 0.0;
00064 }
00065
00066 EthercatHardware::EthercatHardware(const std::string& name) :
00067 hw_(0), node_(ros::NodeHandle(name)),
00068 ni_(0), this_buffer_(0), prev_buffer_(0), buffer_size_(0), halt_motors_(true), reset_state_(0),
00069 max_pd_retries_(10),
00070 diagnostics_publisher_(node_),
00071 motor_publisher_(node_, "motors_halted", 1, true),
00072 device_loader_("ethercat_hardware", "EthercatDevice")
00073 {
00074
00075 }
00076
00077 EthercatHardware::~EthercatHardware()
00078 {
00079 diagnostics_publisher_.stop();
00080 if (slaves_)
00081 {
00082 for (uint32_t i = 0; i < num_slaves_; ++i)
00083 {
00084 EC_FixedStationAddress fsa(i + 1);
00085 EtherCAT_SlaveHandler *sh = em_->get_slave_handler(fsa);
00086 if (sh) sh->to_state(EC_PREOP_STATE);
00087 delete slaves_[i];
00088 }
00089 delete[] slaves_;
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 num_slaves_ = al_->get_num_slaves();
00175 if (num_slaves_ == 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 slaves_ = new EthercatDevice*[num_slaves_];
00191
00192
00193 std::vector<EtherCAT_SlaveHandler*> slave_handles;
00194 for (unsigned int slave = 0; slave < num_slaves_; ++slave)
00195 {
00196 EC_FixedStationAddress fsa(slave + 1);
00197 EtherCAT_SlaveHandler *sh = em_->get_slave_handler(fsa);
00198 if (sh == NULL)
00199 {
00200 ROS_FATAL("Unable to get slave handler #%d", slave);
00201 sleep(1);
00202 exit(EXIT_FAILURE);
00203 }
00204 slave_handles.push_back(sh);
00205 }
00206
00207
00208 BOOST_FOREACH(EtherCAT_SlaveHandler *sh, slave_handles)
00209 {
00210 unsigned slave = sh->get_station_address()-1;
00211 if ((slaves_[slave] = configSlave(sh)) == NULL)
00212 {
00213 ROS_FATAL("Unable to configure slave #%d", slave);
00214 sleep(1);
00215 exit(EXIT_FAILURE);
00216 }
00217 buffer_size_ += slaves_[slave]->command_size_ + slaves_[slave]->status_size_;
00218 }
00219
00220
00221 BOOST_FOREACH(EtherCAT_SlaveHandler *sh, slave_handles)
00222 {
00223 changeState(sh,EC_PREOP_STATE);
00224 }
00225
00226
00227 BOOST_FOREACH(EtherCAT_SlaveHandler *sh, slave_handles)
00228 {
00229 changeState(sh,EC_SAFEOP_STATE);
00230 }
00231
00232
00233
00234 BOOST_FOREACH(EtherCAT_SlaveHandler *sh, slave_handles)
00235 {
00236 changeState(sh,EC_OP_STATE);
00237 }
00238
00239
00240 buffers_ = new unsigned char[2 * buffer_size_];
00241 this_buffer_ = buffers_;
00242 prev_buffer_ = buffers_ + buffer_size_;
00243
00244
00245 memset(this_buffer_, 0, 2 * buffer_size_);
00246 if (!txandrx_PD(buffer_size_, this_buffer_, 20))
00247 {
00248 ROS_FATAL("No communication with devices");
00249 sleep(1);
00250 exit(EXIT_FAILURE);
00251 }
00252
00253
00254 memcpy(prev_buffer_, this_buffer_, buffer_size_);
00255
00256
00257 hw_ = new pr2_hardware_interface::HardwareInterface();
00258 hw_->current_time_ = ros::Time::now();
00259 last_published_ = hw_->current_time_;
00260 motor_last_published_ = hw_->current_time_;
00261
00262
00263
00264 for (unsigned int slave = 0; slave < num_slaves_; ++slave)
00265 {
00266 if (slaves_[slave]->initialize(hw_, allow_unprogrammed) < 0)
00267 {
00268 EtherCAT_SlaveHandler *sh = slaves_[slave]->sh_;
00269 ROS_FATAL("Unable to initialize slave #%d, , product code: %d, revision: %d, serial: %d",
00270 slave, sh->get_product_code(), sh->get_revision(), sh->get_serial());
00271 sleep(1);
00272 exit(EXIT_FAILURE);
00273 }
00274 }
00275
00276
00277 {
00278
00279
00280 static const int MAX_TIMEOUT = 100000;
00281 static const int DEFAULT_TIMEOUT = 20000;
00282 int timeout;
00283 if (!node_.getParam("realtime_socket_timeout", timeout))
00284 {
00285 timeout = DEFAULT_TIMEOUT;
00286 }
00287 if ((timeout <= 1) || (timeout > MAX_TIMEOUT))
00288 {
00289 int old_timeout = timeout;
00290 timeout = std::max(1, std::min(MAX_TIMEOUT, timeout));
00291 ROS_WARN("Invalid timeout (%d) for socket, using %d", old_timeout, timeout);
00292 }
00293 if (set_socket_timeout(ni_, timeout))
00294 {
00295 ROS_FATAL("Error setting socket timeout to %d", timeout);
00296 sleep(1);
00297 exit(EXIT_FAILURE);
00298 }
00299 timeout_ = timeout;
00300
00301
00302
00303
00304
00305
00306
00307
00308
00309
00310 int max_pd_retries = MAX_TIMEOUT / timeout;
00311 static const int MAX_RETRIES=50, MIN_RETRIES=1;
00312 node_.getParam("max_pd_retries", max_pd_retries);
00313
00314 if ((max_pd_retries * timeout) > (MAX_TIMEOUT))
00315 {
00316 max_pd_retries = MAX_TIMEOUT / timeout;
00317 ROS_WARN("Max PD retries is too large for given timeout. Limiting value to %d", max_pd_retries);
00318 }
00319 if ((max_pd_retries < MIN_RETRIES) || (max_pd_retries > MAX_RETRIES))
00320 {
00321 max_pd_retries = std::max(MIN_RETRIES,std::min(MAX_RETRIES,max_pd_retries));
00322 ROS_WARN("Limiting max PD retries to %d", max_pd_retries);
00323 }
00324 max_pd_retries = std::max(MIN_RETRIES,std::min(MAX_RETRIES,max_pd_retries));
00325 max_pd_retries_ = max_pd_retries;
00326 }
00327
00328 diagnostics_publisher_.initialize(interface_, buffer_size_, slaves_, num_slaves_, timeout_, max_pd_retries_);
00329 }
00330
00331
00332 EthercatHardwareDiagnosticsPublisher::EthercatHardwareDiagnosticsPublisher(ros::NodeHandle &node) :
00333 node_(node),
00334 diagnostics_ready_(false),
00335 publisher_(node_.advertise<diagnostic_msgs::DiagnosticArray>("/diagnostics", 1)),
00336 diagnostics_buffer_(NULL),
00337 slaves_(NULL),
00338 num_slaves_(0),
00339 last_dropped_packet_count_(0),
00340 last_dropped_packet_time_(0)
00341 {
00342 }
00343
00344 EthercatHardwareDiagnosticsPublisher::~EthercatHardwareDiagnosticsPublisher()
00345 {
00346 delete[] diagnostics_buffer_;
00347 }
00348
00349 void EthercatHardwareDiagnosticsPublisher::initialize(const string &interface, unsigned int buffer_size, EthercatDevice **slaves, unsigned int num_slaves, unsigned timeout, unsigned max_pd_retries)
00350 {
00351 interface_ = interface;
00352 buffer_size_ = buffer_size;
00353 slaves_ = slaves;
00354 num_slaves_ = num_slaves;
00355 timeout_ = timeout;
00356 max_pd_retries_ = max_pd_retries;
00357
00358 diagnostics_buffer_ = new unsigned char[buffer_size_];
00359
00360
00361 diagnostic_array_.status.reserve(num_slaves_ + 1);
00362 values_.reserve(10);
00363
00364 ethernet_interface_info_.initialize(interface);
00365
00366 diagnostics_thread_ = boost::thread(boost::bind(&EthercatHardwareDiagnosticsPublisher::diagnosticsThreadFunc, this));
00367 }
00368
00369 void EthercatHardwareDiagnosticsPublisher::publish(
00370 const unsigned char *buffer,
00371 const EthercatHardwareDiagnostics &diagnostics)
00372 {
00373 boost::try_to_lock_t try_lock;
00374 boost::unique_lock<boost::mutex> lock(diagnostics_mutex_, try_lock);
00375 if (lock.owns_lock())
00376 {
00377
00378 memcpy(diagnostics_buffer_, buffer, buffer_size_);
00379 diagnostics_ = diagnostics;
00380
00381 diagnostics_ready_ = true;
00382 diagnostics_cond_.notify_one();
00383 }
00384 }
00385
00386 void EthercatHardwareDiagnosticsPublisher::stop()
00387 {
00388 diagnostics_thread_.interrupt();
00389 diagnostics_thread_.join();
00390 publisher_.shutdown();
00391 }
00392
00393 void EthercatHardwareDiagnosticsPublisher::diagnosticsThreadFunc()
00394 {
00395 try {
00396 while (1) {
00397 boost::unique_lock<boost::mutex> lock(diagnostics_mutex_);
00398 while (!diagnostics_ready_) {
00399 diagnostics_cond_.wait(lock);
00400 }
00401 diagnostics_ready_ = false;
00402 publishDiagnostics();
00403 }
00404 } catch (boost::thread_interrupted const&) {
00405 return;
00406 }
00407 }
00408
00409 void EthercatHardwareDiagnosticsPublisher::timingInformation(
00410 diagnostic_updater::DiagnosticStatusWrapper &status,
00411 const string &key,
00412 const accumulator_set<double, stats<tag::max, tag::mean> > &acc,
00413 double max)
00414 {
00415 status.addf(key + " Avg (us)", "%5.4f", extract_result<tag::mean>(acc) * 1e6);
00416 status.addf(key + " 1 Sec Max (us)", "%5.4f", extract_result<tag::max>(acc) * 1e6);
00417 status.addf(key + " Max (us)", "%5.4f", max * 1e6);
00418 }
00419
00420 void EthercatHardwareDiagnosticsPublisher::publishDiagnostics()
00421 {
00422 ros::Time now(ros::Time::now());
00423
00424
00425 status_.clearSummary();
00426 status_.clear();
00427
00428 status_.name = "EtherCAT Master";
00429 if (diagnostics_.motors_halted_)
00430 {
00431 if (diagnostics_.halt_after_reset_)
00432 {
00433 status_.summary(status_.ERROR, "Motors halted soon after reset");
00434 }
00435 else
00436 {
00437 status_.summary(status_.ERROR, "Motors halted");
00438 }
00439 } else {
00440 status_.summary(status_.OK, "OK");
00441 }
00442
00443 if (diagnostics_.pd_error_)
00444 {
00445 status_.mergeSummary(status_.ERROR, "Error sending proccess data");
00446 }
00447
00448 status_.add("Motors halted", diagnostics_.motors_halted_ ? "true" : "false");
00449 status_.addf("EtherCAT devices (expected)", "%d", num_slaves_);
00450 status_.addf("EtherCAT devices (current)", "%d", diagnostics_.device_count_);
00451 ethernet_interface_info_.publishDiagnostics(status_);
00452
00453
00454 status_.addf("Timeout (us)", "%d", timeout_);
00455 status_.addf("Max PD Retries", "%d", max_pd_retries_);
00456
00457
00458 if (num_slaves_ != diagnostics_.device_count_) {
00459 status_.mergeSummary(status_.WARN, "Number of EtherCAT devices changed");
00460 }
00461
00462 timingInformation(status_, "Roundtrip time", diagnostics_.txandrx_acc_, diagnostics_.max_txandrx_);
00463 if (diagnostics_.collect_extra_timing_)
00464 {
00465 timingInformation(status_, "Pack command time", diagnostics_.pack_command_acc_, diagnostics_.max_pack_command_);
00466 timingInformation(status_, "Unpack state time", diagnostics_.unpack_state_acc_, diagnostics_.max_unpack_state_);
00467 timingInformation(status_, "Publish time", diagnostics_.publish_acc_, diagnostics_.max_publish_);
00468 }
00469
00470 status_.addf("EtherCAT Process Data txandrx errors", "%d", diagnostics_.txandrx_errors_);
00471
00472 status_.addf("Reset motors service count", "%d", diagnostics_.reset_motors_service_count_);
00473 status_.addf("Halt motors service count", "%d", diagnostics_.halt_motors_service_count_);
00474 status_.addf("Halt motors error count", "%d", diagnostics_.halt_motors_error_count_);
00475
00476 {
00477 const struct netif_counters *c = &diagnostics_.counters_;
00478 status_.add("Input Thread", (diagnostics_.input_thread_is_stopped_ ? "Stopped" : "Running"));
00479 status_.addf("Sent Packets", "%llu", (unsigned long long)c->sent);
00480 status_.addf("Received Packets", "%llu", (unsigned long long)c->received);
00481 status_.addf("Collected Packets", "%llu", (unsigned long long)c->collected);
00482 status_.addf("Dropped Packets", "%llu", (unsigned long long)c->dropped);
00483 status_.addf("TX Errors", "%llu", (unsigned long long)c->tx_error);
00484 status_.addf("TX Network Down", "%llu", (unsigned long long)c->tx_net_down);
00485 status_.addf("TX Would Block", "%llu", (unsigned long long)c->tx_would_block);
00486 status_.addf("TX No Buffers", "%llu", (unsigned long long)c->tx_no_bufs);
00487 status_.addf("TX Queue Full", "%llu", (unsigned long long)c->tx_full);
00488 status_.addf("RX Runt Packet", "%llu", (unsigned long long)c->rx_runt_pkt);
00489 status_.addf("RX Not EtherCAT", "%llu", (unsigned long long)c->rx_not_ecat);
00490 status_.addf("RX Other EML", "%llu", (unsigned long long)c->rx_other_eml);
00491 status_.addf("RX Bad Index", "%llu", (unsigned long long)c->rx_bad_index);
00492 status_.addf("RX Bad Sequence", "%llu", (unsigned long long)c->rx_bad_seqnum);
00493 status_.addf("RX Duplicate Sequence", "%llu", (unsigned long long)c->rx_dup_seqnum);
00494 status_.addf("RX Duplicate Packet", "%llu", (unsigned long long)c->rx_dup_pkt);
00495 status_.addf("RX Bad Order", "%llu", (unsigned long long)c->rx_bad_order);
00496 status_.addf("RX Late Packet", "%llu", (unsigned long long)c->rx_late_pkt);
00497 status_.addf("RX Late Packet RTT", "%llu", (unsigned long long)c->rx_late_pkt_rtt_us);
00498
00499 double rx_late_pkt_rtt_us_avg = 0.0;
00500 if (c->rx_late_pkt > 0) {
00501 rx_late_pkt_rtt_us_avg = ((double)c->rx_late_pkt_rtt_us_sum)/((double)c->rx_late_pkt);
00502 }
00503 status_.addf("RX Late Packet Avg RTT", "%f", rx_late_pkt_rtt_us_avg);
00504
00505
00506 if (c->dropped > last_dropped_packet_count_)
00507 {
00508 last_dropped_packet_time_ = now;
00509 last_dropped_packet_count_ = c->dropped;
00510 }
00511 }
00512
00513
00514 if ((last_dropped_packet_count_ > 0) && ((now - last_dropped_packet_time_).toSec() < dropped_packet_warning_hold_time_))
00515 {
00516 status_.mergeSummaryf(status_.WARN, "Dropped packets in last %d seconds", dropped_packet_warning_hold_time_);
00517 }
00518
00519 diagnostic_array_.status.clear();
00520 diagnostic_array_.status.push_back(status_);
00521
00522
00523 unsigned char *current = diagnostics_buffer_;
00524 for (unsigned int s = 0; s < num_slaves_; ++s)
00525 {
00526 slaves_[s]->multiDiagnostics(diagnostic_array_.status, current);
00527 current += slaves_[s]->command_size_ + slaves_[s]->status_size_;
00528 }
00529
00530
00531 diagnostic_array_.header.stamp = ros::Time::now();
00532 publisher_.publish(diagnostic_array_);
00533 }
00534
00535 void EthercatHardware::update(bool reset, bool halt)
00536 {
00537
00538 ros::Time update_start_time(ros::Time::now());
00539
00540 unsigned char *this_buffer, *prev_buffer;
00541 bool old_halt_motors = halt_motors_;
00542 bool halt_motors_error = false;
00543
00544
00545 this_buffer = this_buffer_;
00546
00547 if (halt)
00548 {
00549 ++diagnostics_.halt_motors_service_count_;
00550 halt_motors_ = true;
00551 }
00552
00553
00554
00555 const unsigned CYCLES_PER_HALT_RELEASE = 2;
00556 if (reset)
00557 {
00558 ++diagnostics_.reset_motors_service_count_;
00559 reset_state_ = CYCLES_PER_HALT_RELEASE * num_slaves_ + 5;
00560 last_reset_ = update_start_time;
00561 diagnostics_.halt_after_reset_ = false;
00562 }
00563 bool reset_devices = reset_state_ == CYCLES_PER_HALT_RELEASE * num_slaves_ + 3;
00564 if (reset_devices)
00565 {
00566 halt_motors_ = false;
00567 diagnostics_.resetMaxTiming();
00568 diagnostics_.pd_error_ = false;
00569 }
00570
00571 for (unsigned int s = 0; s < num_slaves_; ++s)
00572 {
00573
00574
00575 bool halt_device = halt_motors_ || ((s*CYCLES_PER_HALT_RELEASE+1) < reset_state_);
00576 slaves_[s]->packCommand(this_buffer, halt_device, reset_devices);
00577 this_buffer += slaves_[s]->command_size_ + slaves_[s]->status_size_;
00578 }
00579
00580
00581 ros::Time txandrx_start_time(ros::Time::now());
00582 diagnostics_.pack_command_acc_((txandrx_start_time-update_start_time).toSec());
00583
00584
00585 bool success = txandrx_PD(buffer_size_, this_buffer_, max_pd_retries_);
00586
00587 ros::Time txandrx_end_time(ros::Time::now());
00588 diagnostics_.txandrx_acc_((txandrx_end_time - txandrx_start_time).toSec());
00589
00590 hw_->current_time_ = txandrx_end_time;
00591
00592 if (!success)
00593 {
00594
00595 halt_motors_error = true;
00596 halt_motors_ = true;
00597 diagnostics_.pd_error_ = true;
00598 }
00599 else
00600 {
00601
00602 this_buffer = this_buffer_;
00603 prev_buffer = prev_buffer_;
00604 for (unsigned int s = 0; s < num_slaves_; ++s)
00605 {
00606 if (!slaves_[s]->unpackState(this_buffer, prev_buffer) && !reset_devices)
00607 {
00608 halt_motors_error = true;
00609 halt_motors_ = true;
00610 }
00611 this_buffer += slaves_[s]->command_size_ + slaves_[s]->status_size_;
00612 prev_buffer += slaves_[s]->command_size_ + slaves_[s]->status_size_;
00613 }
00614
00615 if (reset_state_)
00616 --reset_state_;
00617
00618 unsigned char *tmp = this_buffer_;
00619 this_buffer_ = prev_buffer_;
00620 prev_buffer_ = tmp;
00621 }
00622
00623 ros::Time unpack_end_time;
00624 if (diagnostics_.collect_extra_timing_)
00625 {
00626 unpack_end_time = ros::Time::now();
00627 diagnostics_.unpack_state_acc_((unpack_end_time - txandrx_end_time).toSec());
00628 }
00629
00630 if ((update_start_time - last_published_) > ros::Duration(1.0))
00631 {
00632 last_published_ = update_start_time;
00633 publishDiagnostics();
00634 }
00635
00636 if (halt_motors_ != old_halt_motors ||
00637 (update_start_time - motor_last_published_) > ros::Duration(1.0))
00638 {
00639 motor_last_published_ = update_start_time;
00640 motor_publisher_.lock();
00641 motor_publisher_.msg_.data = halt_motors_;
00642 motor_publisher_.unlockAndPublish();
00643 }
00644
00645 if ((halt_motors_ && !old_halt_motors) && (halt_motors_error))
00646 {
00647 ++diagnostics_.halt_motors_error_count_;
00648 if ((update_start_time - last_reset_) < ros::Duration(0.5))
00649 {
00650 diagnostics_.halt_after_reset_ = true;
00651 }
00652 }
00653
00654 if (diagnostics_.collect_extra_timing_)
00655 {
00656 ros::Time publish_end_time(ros::Time::now());
00657 diagnostics_.publish_acc_((publish_end_time - unpack_end_time).toSec());
00658 }
00659 }
00660
00661 void EthercatHardware::updateAccMax(double &max, const accumulator_set<double, stats<tag::max, tag::mean> > &acc)
00662 {
00663 max = std::max(max, extract_result<tag::max>(acc));
00664 }
00665
00666 void EthercatHardware::publishDiagnostics()
00667 {
00668
00669 updateAccMax(diagnostics_.max_pack_command_, diagnostics_.pack_command_acc_);
00670 updateAccMax(diagnostics_.max_txandrx_, diagnostics_.txandrx_acc_);
00671 updateAccMax(diagnostics_.max_unpack_state_, diagnostics_.unpack_state_acc_);
00672 updateAccMax(diagnostics_.max_publish_, diagnostics_.publish_acc_);
00673
00674
00675 diagnostics_.counters_ = ni_->counters;
00676 diagnostics_.input_thread_is_stopped_ = bool(ni_->is_stopped);
00677
00678 diagnostics_.motors_halted_ = halt_motors_;
00679
00680
00681 diagnostics_publisher_.publish(this_buffer_, diagnostics_);
00682
00683
00684 static accumulator_set<double, stats<tag::max, tag::mean> > blank;
00685 diagnostics_.pack_command_acc_ = blank;
00686 diagnostics_.txandrx_acc_ = blank;
00687 diagnostics_.unpack_state_acc_ = blank;
00688 diagnostics_.publish_acc_ = blank;
00689 }
00690
00691
00692 EthercatDevice *
00693 EthercatHardware::configSlave(EtherCAT_SlaveHandler *sh)
00694 {
00695 static int start_address = 0x00010000;
00696 EthercatDevice *p = NULL;
00697 stringstream str;
00698 unsigned product_code = sh->get_product_code();
00699 unsigned serial = sh->get_serial();
00700 uint32_t revision = sh->get_revision();
00701 unsigned slave = sh->get_station_address()-1;
00702
00703 str << product_code;
00704 try {
00705 p = device_loader_.createClassInstance(str.str());
00706 }
00707 catch (pluginlib::LibraryLoadException &e)
00708 {
00709 p = NULL;
00710 ROS_FATAL("Unable to load plugin for slave #%d, product code: %u (0x%X), serial: %u (0x%X), revision: %d (0x%X)",
00711 slave, product_code, product_code, serial, serial, revision, revision);
00712 if ((product_code==0xbaddbadd) || (serial==0xbaddbadd) || (revision==0xbaddbadd))
00713 {
00714 ROS_FATAL("Note: 0xBADDBADD indicates that the value was not read correctly from device.");
00715 ROS_FATAL("Perhaps you should power-cycle the MCBs");
00716 }
00717 else
00718 {
00719 ROS_FATAL("%s", e.what());
00720 }
00721 }
00722 if (p) {
00723 p->construct(sh, start_address);
00724 }
00725 return p;
00726 }
00727
00728
00729 void EthercatHardware::collectDiagnostics()
00730 {
00731 if (NULL == oob_com_)
00732 return;
00733
00734 {
00735 EC_Logic *logic = EC_Logic::instance();
00736 unsigned char p[1];
00737 EC_UINT length = sizeof(p);
00738
00739
00740 APRD_Telegram status(logic->get_idx(),
00741 0,
00742 0,
00743 logic->get_wkc(),
00744 length,
00745 p);
00746
00747
00748 EC_Ethernet_Frame frame(&status);
00749 oob_com_->txandrx(&frame);
00750
00751
00752 diagnostics_.device_count_ = status.get_adp();
00753 }
00754
00755 for (unsigned i = 0; i < num_slaves_; ++i)
00756 {
00757 EthercatDevice * d(slaves_[i]);
00758 d->collectDiagnostics(oob_com_);
00759 }
00760 }
00761
00762
00763
00764 void EthercatHardware::printCounters(std::ostream &os)
00765 {
00766 const struct netif_counters &c(ni_->counters);
00767 os << "netif counters :" << endl
00768 << " sent = " << c.sent << endl
00769 << " received = " << c.received << endl
00770 << " collected = " << c.collected << endl
00771 << " dropped = " << c.dropped << endl
00772 << " tx_error = " << c.tx_error << endl
00773 << " tx_net_down = " << c.tx_net_down << endl
00774 << " tx_would_block= " << c.tx_would_block << endl
00775 << " tx_no_bufs = " << c.tx_no_bufs << endl
00776 << " tx_full = " << c.tx_full << endl
00777 << " rx_runt_pkt = " << c.rx_runt_pkt << endl
00778 << " rx_not_ecat = " << c.rx_not_ecat << endl
00779 << " rx_other_eml = " << c.rx_other_eml << endl
00780 << " rx_bad_index = " << c.rx_bad_index << endl
00781 << " rx_bad_seqnum = " << c.rx_bad_seqnum << endl
00782 << " rx_dup_seqnum = " << c.rx_dup_seqnum << endl
00783 << " rx_dup_pkt = " << c.rx_dup_pkt << endl
00784 << " rx_bad_order = " << c.rx_bad_order << endl
00785 << " rx_late_pkt = " << c.rx_late_pkt << endl;
00786 }
00787
00788
00789 bool EthercatHardware::txandrx_PD(unsigned buffer_size, unsigned char* buffer, unsigned tries)
00790 {
00791
00792 bool success = false;
00793 for (unsigned i=0; i<tries && !success; ++i) {
00794
00795 success = em_->txandrx_PD(buffer_size_, this_buffer_);
00796 if (!success) {
00797 ++diagnostics_.txandrx_errors_;
00798 }
00799
00800 oob_com_->tx();
00801 }
00802 return success;
00803 }
00804
00805
00806 bool EthercatHardware::publishTrace(int position, const string &reason, unsigned level, unsigned delay)
00807 {
00808 if (position >= (int)num_slaves_)
00809 {
00810 ROS_WARN("Invalid device position %d. Use 0-%d, or -1.", position, num_slaves_-1);
00811 return false;
00812 }
00813
00814 if (level > 2)
00815 {
00816 ROS_WARN("Invalid level : %d. Using level=2 (ERROR).", level);
00817 level = 2;
00818 }
00819
00820 string new_reason("Manually triggered : " + reason);
00821
00822 bool retval = false;
00823 if (position < 0)
00824 {
00825 for (unsigned i=0; i<num_slaves_; ++i)
00826 {
00827 if (slaves_[i]->publishTrace(new_reason,level,delay))
00828 {
00829 retval = true;
00830 }
00831 }
00832 }
00833 else
00834 {
00835 retval = slaves_[position]->publishTrace(new_reason,level,delay);
00836 if (!retval)
00837 {
00838 ROS_WARN("Device %d does not support publishing trace", position);
00839 }
00840 }
00841 return retval;
00842 }