ethercat_hardware.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 <vector>
36 
38 
39 #include <ethercat/ethercat_xenomai_drv.h>
40 #include <dll/ethercat_dll.h>
41 #include <dll/ethercat_device_addressed_telegram.h>
42 
43 #include <sstream>
44 
45 #include <net/if.h>
46 #include <sys/ioctl.h>
47 #include <boost/bind.hpp>
48 #include <boost/foreach.hpp>
49 #include <boost/regex.hpp>
50 #include <boost/shared_ptr.hpp>
51 #include <boost/thread/thread.hpp>
52 
54 
55  txandrx_errors_(0),
56  device_count_(0),
57  pd_error_(false),
58  halt_after_reset_(false),
59  reset_motors_service_count_(0),
60  halt_motors_service_count_(0),
61  halt_motors_error_count_(0),
62  motors_halted_(false),
63  motors_halted_reason_("")
64 {
66 }
67 
69 {
70  max_pack_command_ = 0.0;
71  max_txandrx_ = 0.0;
72  max_unpack_state_ = 0.0;
73  max_publish_ = 0.0;
74 }
75 
76 EthercatHardware::EthercatHardware(const std::string& name) :
77  hw_(0), node_(ros::NodeHandle(name)),
78  ni_(0), this_buffer_(0), prev_buffer_(0), buffer_size_(0), halt_motors_(true), reset_state_(0),
79  max_pd_retries_(10),
80  diagnostics_publisher_(node_),
81  motor_publisher_(node_, "motors_halted", 1, true),
82  device_loader_("ethercat_hardware", "EthercatDevice")
83 {
84 
85 }
86 
88 {
90  for (uint32_t i = 0; i < slaves_.size(); ++i)
91  {
92  EC_FixedStationAddress fsa(i + 1);
93  EtherCAT_SlaveHandler *sh = em_->get_slave_handler(fsa);
94  if (sh) sh->to_state(EC_PREOP_STATE);
95  }
96  if (ni_)
97  {
99  }
100  delete[] buffers_;
101  delete hw_;
102  delete oob_com_;
104 }
105 
106 
107 void EthercatHardware::changeState(EtherCAT_SlaveHandler *sh, EC_State new_state)
108 {
109  unsigned product_code = sh->get_product_code();
110  unsigned serial = sh->get_serial();
111  uint32_t revision = sh->get_revision();
112  unsigned slave = sh->get_station_address()-1;
113 
114  if (!sh->to_state(new_state))
115  {
116  ROS_FATAL("Cannot goto state %d for slave #%d, product code: %u (0x%X), serial: %u (0x%X), revision: %d (0x%X)",
117  new_state, slave, product_code, product_code, serial, serial, revision, revision);
118  if ((product_code==0xbaddbadd) || (serial==0xbaddbadd) || (revision==0xbaddbadd))
119  ROS_FATAL("Note: 0xBADDBADD indicates that the value was not read correctly from device.");
120  exit(EXIT_FAILURE);
121  }
122 }
123 
124 void EthercatHardware::init(char *interface, bool allow_unprogrammed)
125 {
126  // open temporary socket to use with ioctl
127  int sock = socket(PF_INET, SOCK_DGRAM, 0);
128  if (sock < 0) {
129  int error = errno;
130  ROS_FATAL("Couldn't open temp socket : %s", strerror(error));
131  sleep(1);
132  exit(EXIT_FAILURE);
133  }
134 
135  struct ifreq ifr;
136  strncpy(ifr.ifr_name, interface, IFNAMSIZ);
137  if (ioctl(sock, SIOCGIFFLAGS, &ifr) < 0) {
138  int error = errno;
139  ROS_FATAL("Cannot get interface flags for %s: %s", interface, strerror(error));
140  sleep(1);
141  exit(EXIT_FAILURE);
142  }
143 
144  close(sock);
145  sock = -1;
146 
147  if (!(ifr.ifr_flags & IFF_UP)) {
148  ROS_FATAL("Interface %s is not UP. Try : ifup %s", interface, interface);
149  sleep(1);
150  exit(EXIT_FAILURE);
151  }
152  if (!(ifr.ifr_flags & IFF_RUNNING)) {
153  ROS_FATAL("Interface %s is not RUNNING. Is cable plugged in and device powered?", interface);
154  sleep(1);
155  exit(EXIT_FAILURE);
156  }
157 
158 
159  // Initialize network interface
160  interface_ = interface;
161  if ((ni_ = init_ec(interface)) == NULL)
162  {
163  ROS_FATAL("Unable to initialize interface: %s", interface);
164  sleep(1);
165  exit(EXIT_FAILURE);
166  }
167 
168  oob_com_ = new EthercatOobCom(ni_);
169 
170  // Initialize Application Layer (AL)
171  EtherCAT_DataLinkLayer::instance()->attach(ni_);
172  if ((al_ = EtherCAT_AL::instance()) == NULL)
173  {
174  ROS_FATAL("Unable to initialize Application Layer (AL): %p", al_);
175  sleep(1);
176  exit(EXIT_FAILURE);
177  }
178 
179  int num_ethercat_devices_ = al_->get_num_slaves();
180  if (num_ethercat_devices_ == 0)
181  {
182  ROS_FATAL("Unable to locate any slaves");
183  sleep(1);
184  exit(EXIT_FAILURE);
185  }
186 
187  // Initialize Master
188  if ((em_ = EtherCAT_Master::instance()) == NULL)
189  {
190  ROS_FATAL("Unable to initialize EtherCAT_Master: %p", em_);
191  sleep(1);
192  exit(EXIT_FAILURE);
193  }
194 
195  // Size slaves vector to hold appropriate number of device pointers
196  slaves_.resize(num_ethercat_devices_);
197 
198  // Make temporary list of slave handles
199  std::vector<EtherCAT_SlaveHandler*> slave_handles;
200  for (unsigned int slave = 0; slave < slaves_.size(); ++slave)
201  {
202  EC_FixedStationAddress fsa(slave + 1);
203  EtherCAT_SlaveHandler *sh = em_->get_slave_handler(fsa);
204  if (sh == NULL)
205  {
206  ROS_FATAL("Unable to get slave handler #%d", slave);
207  sleep(1);
208  exit(EXIT_FAILURE);
209  }
210  slave_handles.push_back(sh);
211  }
212 
213  // Configure EtherCAT slaves
214  BOOST_FOREACH(EtherCAT_SlaveHandler *sh, slave_handles)
215  {
216  unsigned slave = sh->get_station_address()-1;
217  if ((slaves_[slave] = configSlave(sh)) == NULL)
218  {
219  ROS_FATAL("Unable to configure slave #%d", slave);
220  sleep(1);
221  exit(EXIT_FAILURE);
222  }
223  buffer_size_ += slaves_[slave]->command_size_ + slaves_[slave]->status_size_;
224  }
225 
226  // Configure any non-ethercat slaves (appends devices to slaves_ vector)
228 
229  // Move slave from INIT to PREOP
230  BOOST_FOREACH(EtherCAT_SlaveHandler *sh, slave_handles)
231  {
232  changeState(sh,EC_PREOP_STATE);
233  }
234 
235  // Move slave from PREOP to SAFEOP
236  BOOST_FOREACH(EtherCAT_SlaveHandler *sh, slave_handles)
237  {
238  changeState(sh,EC_SAFEOP_STATE);
239  }
240 
241  // Move slave from SAFEOP to OP
242  // TODO : move to OP after initializing slave process data
243  BOOST_FOREACH(EtherCAT_SlaveHandler *sh, slave_handles)
244  {
245  changeState(sh,EC_OP_STATE);
246  }
247 
248  // Allocate buffers to send and receive commands
249  buffers_ = new unsigned char[2 * buffer_size_];
252 
253  // Make sure motors are disabled, also collect status data
254  memset(this_buffer_, 0, 2 * buffer_size_);
255  if (!txandrx_PD(buffer_size_, this_buffer_, 20))
256  {
257  ROS_FATAL("No communication with devices");
258  sleep(1);
259  exit(EXIT_FAILURE);
260  }
261 
262  // prev_buffer should contain valid status data when update function is first used
263  memcpy(prev_buffer_, this_buffer_, buffer_size_);
264 
265  // Create pr2_hardware_interface::HardwareInterface
269 
270  // Initialize slaves
271  //set<string> actuator_names;
272  for (unsigned int slave = 0; slave < slaves_.size(); ++slave)
273  {
274  if (slaves_[slave]->initialize(hw_, allow_unprogrammed) < 0)
275  {
276  EtherCAT_SlaveHandler *sh = slaves_[slave]->sh_;
277  if (sh != NULL)
278  {
279  ROS_FATAL("Unable to initialize slave #%d, product code: %d, revision: %d, serial: %d",
280  slave, sh->get_product_code(), sh->get_revision(), sh->get_serial());
281  sleep(1);
282  }
283  else
284  {
285  ROS_FATAL("Unable to initialize slave #%d", slave);
286  }
287  exit(EXIT_FAILURE);
288  }
289  }
290 
291 
292  { // Initialization is now complete. Reduce timeout of EtherCAT txandrx for better realtime performance
293  // Allow timeout to be configured at program load time with rosparam.
294  // This will allow tweaks for systems with different realtime performace
295  static const int MAX_TIMEOUT = 100000; // 100ms = 100,000us
296  static const int DEFAULT_TIMEOUT = 20000; // default to timeout to 20000us = 20ms
297  int timeout;
298  if (!node_.getParam("realtime_socket_timeout", timeout))
299  {
300  timeout = DEFAULT_TIMEOUT;
301  }
302  if ((timeout <= 1) || (timeout > MAX_TIMEOUT))
303  {
304  int old_timeout = timeout;
305  timeout = std::max(1, std::min(MAX_TIMEOUT, timeout));
306  ROS_WARN("Invalid timeout (%d) for socket, using %d", old_timeout, timeout);
307  }
308  if (set_socket_timeout(ni_, timeout))
309  {
310  ROS_FATAL("Error setting socket timeout to %d", timeout);
311  sleep(1);
312  exit(EXIT_FAILURE);
313  }
314  timeout_ = timeout;
315 
316  // When packet constaining process data is does not return after a given timeout, it is
317  // assumed to be dropped and the process data will automatically get re-sent.
318  // After a number of retries, the driver will halt motors as a safety precaution.
319  //
320  // The following code allows the number of process data retries to be changed with a rosparam.
321  // This is needed because lowering the txandrx timeout makes it more likely that a
322  // performance hickup in network or OS causes will cause the motors to halt.
323  //
324  // If number of retries is not specified, use a formula that allows 100ms of dropped packets
325  int max_pd_retries = MAX_TIMEOUT / timeout; // timeout is in nanoseconds : 20msec = 20000usec
326  static const int MAX_RETRIES=50, MIN_RETRIES=1;
327  node_.getParam("max_pd_retries", max_pd_retries);
328  // Make sure motor halt due to dropped packet takes less than 1/10 of a second
329  if ((max_pd_retries * timeout) > (MAX_TIMEOUT))
330  {
331  max_pd_retries = MAX_TIMEOUT / timeout;
332  ROS_WARN("Max PD retries is too large for given timeout. Limiting value to %d", max_pd_retries);
333  }
334  if ((max_pd_retries < MIN_RETRIES) || (max_pd_retries > MAX_RETRIES))
335  {
336  max_pd_retries = std::max(MIN_RETRIES,std::min(MAX_RETRIES,max_pd_retries));
337  ROS_WARN("Limiting max PD retries to %d", max_pd_retries);
338  }
339  max_pd_retries = std::max(MIN_RETRIES,std::min(MAX_RETRIES,max_pd_retries));
340  max_pd_retries_ = max_pd_retries;
341  }
342 
343  diagnostics_publisher_.initialize(interface_, buffer_size_, slaves_, num_ethercat_devices_, timeout_, max_pd_retries_);
344 }
345 
346 
348  node_(node),
349  diagnostics_ready_(false),
350  publisher_(node_.advertise<diagnostic_msgs::DiagnosticArray>("/diagnostics", 1)),
351  diagnostics_buffer_(NULL),
352  last_dropped_packet_count_(0),
353  last_dropped_packet_time_(0)
354 {
355 }
356 
358 {
359  delete[] diagnostics_buffer_;
360 }
361 
362 void EthercatHardwareDiagnosticsPublisher::initialize(const string &interface, unsigned int buffer_size,
363  const std::vector<boost::shared_ptr<EthercatDevice> > &slaves,
364  unsigned int num_ethercat_devices,
365  unsigned timeout, unsigned max_pd_retries)
366 {
367  interface_ = interface;
368  buffer_size_ = buffer_size;
369  slaves_ = slaves;
370  num_ethercat_devices_ = num_ethercat_devices;
371  timeout_ = timeout;
372  max_pd_retries_ = max_pd_retries;
373 
374  diagnostics_buffer_ = new unsigned char[buffer_size_];
375 
376  // Initialize diagnostic data structures
377  diagnostic_array_.status.reserve(slaves_.size() + 1);
378  values_.reserve(10);
379 
381 
383 }
384 
386  const unsigned char *buffer,
387  const EthercatHardwareDiagnostics &diagnostics)
388 {
389  boost::try_to_lock_t try_lock;
390  boost::unique_lock<boost::mutex> lock(diagnostics_mutex_, try_lock);
391  if (lock.owns_lock())
392  {
393  // Make copies of diagnostic data for dianostic thread
394  memcpy(diagnostics_buffer_, buffer, buffer_size_);
395  diagnostics_ = diagnostics;
396  // Trigger diagnostics publish thread
397  diagnostics_ready_ = true;
398  diagnostics_cond_.notify_one();
399  }
400 }
401 
403 {
404  diagnostics_thread_.interrupt();
405  diagnostics_thread_.join();
407 }
408 
410 {
411  try {
412  while (1) {
413  boost::unique_lock<boost::mutex> lock(diagnostics_mutex_);
414  while (!diagnostics_ready_) {
415  diagnostics_cond_.wait(lock);
416  }
417  diagnostics_ready_ = false;
419  }
420  } catch (boost::thread_interrupted const&) {
421  return;
422  }
423 }
424 
427  const string &key,
428  const accumulator_set<double, stats<tag::max, tag::mean> > &acc,
429  double max)
430 {
431  status.addf(key + " Avg (us)", "%5.4f", extract_result<tag::mean>(acc) * 1e6); // Average over last 1 second
432  status.addf(key + " 1 Sec Max (us)", "%5.4f", extract_result<tag::max>(acc) * 1e6); // Max over last 1 second
433  status.addf(key + " Max (us)", "%5.4f", max * 1e6); // Max since start
434 }
435 
437 {
438  ros::Time now(ros::Time::now());
439 
440  // Publish status of EtherCAT master
442  status_.clear();
443 
444  status_.name = "EtherCAT Master";
446  {
447  std::ostringstream desc;
448  desc << "Motors halted";
450  {
451  desc << " soon after reset";
452  }
453  desc << " (" << diagnostics_.motors_halted_reason_ << ")";
454  status_.summary(status_.ERROR, desc.str());
455  } else {
456  status_.summary(status_.OK, "OK");
457  }
458 
460  {
461  status_.mergeSummary(status_.ERROR, "Error sending proccess data");
462  }
463 
464  status_.add("Motors halted", diagnostics_.motors_halted_ ? "true" : "false");
465  status_.addf("EtherCAT devices (expected)", "%d", num_ethercat_devices_);
466  status_.addf("EtherCAT devices (current)", "%d", diagnostics_.device_count_);
468  //status_.addf("Reset state", "%d", reset_state_);
469 
470  status_.addf("Timeout (us)", "%d", timeout_);
471  status_.addf("Max PD Retries", "%d", max_pd_retries_);
472 
473  // Produce warning if number of devices changed after device initalization
475  status_.mergeSummary(status_.WARN, "Number of EtherCAT devices changed");
476  }
477 
480  {
484  }
485 
486  status_.addf("EtherCAT Process Data txandrx errors", "%d", diagnostics_.txandrx_errors_);
487 
488  status_.addf("Reset motors service count", "%d", diagnostics_.reset_motors_service_count_);
489  status_.addf("Halt motors service count", "%d", diagnostics_.halt_motors_service_count_);
490  status_.addf("Halt motors error count", "%d", diagnostics_.halt_motors_error_count_);
491 
492  { // Publish ethercat network interface counters
493  const struct netif_counters *c = &diagnostics_.counters_;
494  status_.add("Input Thread", (diagnostics_.input_thread_is_stopped_ ? "Stopped" : "Running"));
495  status_.addf("Sent Packets", "%llu", (unsigned long long)c->sent);
496  status_.addf("Received Packets", "%llu", (unsigned long long)c->received);
497  status_.addf("Collected Packets", "%llu", (unsigned long long)c->collected);
498  status_.addf("Dropped Packets", "%llu", (unsigned long long)c->dropped);
499  status_.addf("TX Errors", "%llu", (unsigned long long)c->tx_error);
500  status_.addf("TX Network Down", "%llu", (unsigned long long)c->tx_net_down);
501  status_.addf("TX Would Block", "%llu", (unsigned long long)c->tx_would_block);
502  status_.addf("TX No Buffers", "%llu", (unsigned long long)c->tx_no_bufs);
503  status_.addf("TX Queue Full", "%llu", (unsigned long long)c->tx_full);
504  status_.addf("RX Runt Packet", "%llu", (unsigned long long)c->rx_runt_pkt);
505  status_.addf("RX Not EtherCAT", "%llu", (unsigned long long)c->rx_not_ecat);
506  status_.addf("RX Other EML", "%llu", (unsigned long long)c->rx_other_eml);
507  status_.addf("RX Bad Index", "%llu", (unsigned long long)c->rx_bad_index);
508  status_.addf("RX Bad Sequence", "%llu", (unsigned long long)c->rx_bad_seqnum);
509  status_.addf("RX Duplicate Sequence", "%llu", (unsigned long long)c->rx_dup_seqnum);
510  status_.addf("RX Duplicate Packet", "%llu", (unsigned long long)c->rx_dup_pkt);
511  status_.addf("RX Bad Order", "%llu", (unsigned long long)c->rx_bad_order);
512  status_.addf("RX Late Packet", "%llu", (unsigned long long)c->rx_late_pkt);
513  status_.addf("RX Late Packet RTT", "%llu", (unsigned long long)c->rx_late_pkt_rtt_us);
514 
515  double rx_late_pkt_rtt_us_avg = 0.0;
516  if (c->rx_late_pkt > 0) {
517  rx_late_pkt_rtt_us_avg = ((double)c->rx_late_pkt_rtt_us_sum)/((double)c->rx_late_pkt);
518  }
519  status_.addf("RX Late Packet Avg RTT", "%f", rx_late_pkt_rtt_us_avg);
520 
521  // Check for newly dropped packets
522  if (c->dropped > last_dropped_packet_count_)
523  {
525  last_dropped_packet_count_ = c->dropped;
526  }
527  }
528 
529  // Create error message if packet has been dropped recently
531  {
532  status_.mergeSummaryf(status_.WARN, "Dropped packets in last %d seconds", dropped_packet_warning_hold_time_);
533  }
534 
535  diagnostic_array_.status.clear();
536  diagnostic_array_.status.push_back(status_);
537 
538  // Also, collect diagnostic statuses of all EtherCAT device
539  unsigned char *current = diagnostics_buffer_;
540  for (unsigned int s = 0; s < slaves_.size(); ++s)
541  {
542  slaves_[s]->multiDiagnostics(diagnostic_array_.status, current);
543  current += slaves_[s]->command_size_ + slaves_[s]->status_size_;
544  }
545 
546  // Publish status of each EtherCAT device
547  diagnostic_array_.header.stamp = ros::Time::now();
549 }
550 
551 void EthercatHardware::update(bool reset, bool halt)
552 {
553  // Update current time
554  ros::Time update_start_time(ros::Time::now());
555 
556  unsigned char *this_buffer, *prev_buffer;
557 
558  // Convert HW Interface commands to MCB-specific buffers
559  this_buffer = this_buffer_;
560 
561  if (halt)
562  {
564  haltMotors(false /*no error*/, "service request");
565  }
566 
567  // Resetting devices should clear device errors and release devices from halt.
568  // To reduce load on power system, release devices from halt, one at a time
569  const unsigned CYCLES_PER_HALT_RELEASE = 2; // Wait two cycles between releasing each device
570  if (reset)
571  {
573  reset_state_ = CYCLES_PER_HALT_RELEASE * slaves_.size() + 5;
574  last_reset_ = update_start_time;
576  }
577  bool reset_devices = reset_state_ == CYCLES_PER_HALT_RELEASE * slaves_.size() + 3;
578  if (reset_devices)
579  {
580  halt_motors_ = false;
584  diagnostics_.pd_error_ = false;
585  }
586 
587  for (unsigned int s = 0; s < slaves_.size(); ++s)
588  {
589  // Pack the command structures into the EtherCAT buffer
590  // Disable the motor if they are halted or coming out of reset
591  bool halt_device = halt_motors_ || ((s*CYCLES_PER_HALT_RELEASE+1) < reset_state_);
592  slaves_[s]->packCommand(this_buffer, halt_device, reset_devices);
593  this_buffer += slaves_[s]->command_size_ + slaves_[s]->status_size_;
594  }
595 
596  // Transmit process data
597  ros::Time txandrx_start_time(ros::Time::now()); // Also end time for pack_command_stage
598  diagnostics_.pack_command_acc_((txandrx_start_time-update_start_time).toSec());
599 
600  // Send/receive device proccess data
601  bool success = txandrx_PD(buffer_size_, this_buffer_, max_pd_retries_);
602 
603  ros::Time txandrx_end_time(ros::Time::now()); // Also begining of unpack_state
604  diagnostics_.txandrx_acc_((txandrx_end_time - txandrx_start_time).toSec());
605 
606  hw_->current_time_ = txandrx_end_time;
607 
608  if (!success)
609  {
610  // If process data didn't get sent after multiple retries, stop motors
611  haltMotors(true /*error*/, "communication error");
612  diagnostics_.pd_error_ = true;
613  }
614  else
615  {
616  // Convert status back to HW Interface
617  this_buffer = this_buffer_;
618  prev_buffer = prev_buffer_;
619  for (unsigned int s = 0; s < slaves_.size(); ++s)
620  {
621  if (!slaves_[s]->unpackState(this_buffer, prev_buffer) && !reset_devices)
622  {
623  haltMotors(true /*error*/, "device error");
624  }
625  this_buffer += slaves_[s]->command_size_ + slaves_[s]->status_size_;
626  prev_buffer += slaves_[s]->command_size_ + slaves_[s]->status_size_;
627  }
628 
629  if (reset_state_)
630  --reset_state_;
631 
632  unsigned char *tmp = this_buffer_;
633  this_buffer_ = prev_buffer_;
634  prev_buffer_ = tmp;
635  }
636 
637  ros::Time unpack_end_time;
639  {
640  unpack_end_time = ros::Time::now(); // also start of publish time
641  diagnostics_.unpack_state_acc_((unpack_end_time - txandrx_end_time).toSec());
642  }
643 
644  if ((update_start_time - last_published_) > ros::Duration(1.0))
645  {
646  last_published_ = update_start_time;
648  motor_publisher_.lock();
649  motor_publisher_.msg_.data = halt_motors_;
650  motor_publisher_.unlockAndPublish();
651  }
652 
654  {
655  ros::Time publish_end_time(ros::Time::now());
656  diagnostics_.publish_acc_((publish_end_time - unpack_end_time).toSec());
657  }
658 }
659 
660 
661 void EthercatHardware::haltMotors(bool error, const char* reason)
662 {
663  if (!halt_motors_)
664  {
665  // wasn't already halted
666  motor_publisher_.lock();
667  motor_publisher_.msg_.data = halt_motors_;
668  motor_publisher_.unlockAndPublish();
669 
671  if (error)
672  {
674  if ((ros::Time::now() - last_reset_) < ros::Duration(0.5))
675  {
676  // halted soon after reset
678  }
679  }
680  }
681 
683  halt_motors_ = true;
684 }
685 
686 
687 void EthercatHardware::updateAccMax(double &max, const accumulator_set<double, stats<tag::max, tag::mean> > &acc)
688 {
689  max = std::max(max, extract_result<tag::max>(acc));
690 }
691 
693 {
694  // Update max timing values
699 
700  // Grab stats and counters from input thread
701  diagnostics_.counters_ = ni_->counters;
702  diagnostics_.input_thread_is_stopped_ = bool(ni_->is_stopped);
703 
704  diagnostics_.motors_halted_ = halt_motors_;
705 
706  // Pass diagnostic data to publisher thread
707  diagnostics_publisher_.publish(this_buffer_, diagnostics_);
708 
709  // Clear statistics accumulators
710  static accumulator_set<double, stats<tag::max, tag::mean> > blank;
712  diagnostics_.txandrx_acc_ = blank;
714  diagnostics_.publish_acc_ = blank;
715 }
716 
717 
719 EthercatHardware::configSlave(EtherCAT_SlaveHandler *sh)
720 {
721  static int start_address = 0x00010000;
723  unsigned product_code = sh->get_product_code();
724  unsigned serial = sh->get_serial();
725  uint32_t revision = sh->get_revision();
726  unsigned slave = sh->get_station_address()-1;
727 
728  // The point of this code to find a class whose name matches the EtherCAT
729  // product ID for a given device.
730  // Thus device plugins would register themselves with PLUGIN_REGISTER_CLASS
731  //
732  // PLUGINLIB_EXPORT_CLASS(class_type, base_class_type)
733  //
734  // and in the plugin.xml, specify name="" inside the <class> tag:
735  //
736  // <class name="package/serial"
737  // base_class_type="ethercat_hardware::EthercatDevice" />
738  //
739  //
740  // For the WG05 driver (productID = 6805005), this statement would look
741  // something like:
742  //
743  // PLUGINLIB_EXPORT_CLASS(WG05, EthercatDevice)
744  //
745  // and in the plugin.xml:
746  //
747  // <class name="ethercat_hardware/6805005" type="WG05"
748  // base_class_type="EthercatDevice">
749  // <description>
750  // WG05 - Generic Motor Control Board
751  // </description>
752  // </class>
753  //
754  //
755  // Unfortunately, we don't know which ROS package that a particular driver is defined in.
756  // To account for this, we search through the list of class names, one-by-one and find string where
757  // last part of string matches product ID of device.
758  stringstream class_name_regex_str;
759  class_name_regex_str << "(.*/)?" << product_code;
760  boost::regex class_name_regex(class_name_regex_str.str(), boost::regex::extended);
761 
762  std::vector<std::string> classes = device_loader_.getDeclaredClasses();
763  std::string matching_class_name;
764 
765  BOOST_FOREACH(const std::string &class_name, classes)
766  {
767  if (regex_match(class_name, class_name_regex))
768  {
769  if (matching_class_name.size() != 0)
770  {
771  ROS_ERROR("Found more than 1 EtherCAT driver for device with product code : %d", product_code);
772  ROS_ERROR("First class name = '%s'. Second class name = '%s'",
773  class_name.c_str(), matching_class_name.c_str());
774  }
775  matching_class_name = class_name;
776  }
777  }
778 
779  if (matching_class_name.size() != 0)
780  {
781  //ROS_WARN("Using driver class '%s' for device with product code %d",
782  // matching_class_name.c_str(), product_code);
783  try {
784  p = device_loader_.createInstance(matching_class_name);
785  }
787  {
788  p.reset();
789  ROS_FATAL("Unable to load plugin for slave #%d, product code: %u (0x%X), serial: %u (0x%X), revision: %d (0x%X)",
790  slave, product_code, product_code, serial, serial, revision, revision);
791  ROS_FATAL("%s", e.what());
792  }
793  }
794  else
795  {
796  if ((product_code==0xbaddbadd) || (serial==0xbaddbadd) || (revision==0xbaddbadd))
797  {
798  ROS_FATAL("Note: 0xBADDBADD indicates that the value was not read correctly from device.");
799  ROS_FATAL("Perhaps you should power-cycle the MCBs");
800  }
801  else
802  {
803  ROS_ERROR("Unable to load plugin for slave #%d, product code: %u (0x%X), serial: %u (0x%X), revision: %d (0x%X)",
804  slave, product_code, product_code, serial, serial, revision, revision);
805  ROS_ERROR("Possible classes:");
806  BOOST_FOREACH(const std::string &class_name, classes)
807  {
808  ROS_ERROR(" %s", class_name.c_str());
809  }
810 
811  // TODO, use default plugin for ethercat devices that have no driver.
812  // This way, the EtherCAT chain still works.
813  }
814  }
815 
816  if (p != NULL)
817  {
818  p->construct(sh, start_address);
819  }
820 
821  return p;
822 }
823 
824 
826 EthercatHardware::configNonEthercatDevice(const std::string &name, const std::string &type)
827 {
829  try {
830  p = device_loader_.createInstance(type);
831  }
833  {
834  p.reset();
835  ROS_FATAL("Unable to load plugin for non-EtherCAT device '%s' with type: %s : %s"
836  , name.c_str(), type.c_str(), e.what());
837  }
838  if (p) {
839  ROS_INFO("Creating non-EtherCAT device '%s' of type '%s'", name.c_str(), type.c_str());
840  ros::NodeHandle nh(node_, "non_ethercat_devices/"+name);
841  p->construct(nh);
842  }
843  return p;
844 }
845 
846 
847 // Do this to get access to Struct (std::map) element of XmlRpcValue
849 {
850 public:
851  MyXmlRpcValue(XmlRpc::XmlRpcValue &value) : XmlRpc::XmlRpcValue(value) { }
852  XmlRpcValue::ValueStruct &getMap() {return *_value.asStruct;}
853 };
854 
856 {
857  // non-EtherCAT device drivers are descibed via struct named "non_ethercat_devices"
858  // Each element of "non_ethercat_devices" must be a struct that contains a "type" field.
859  // The element struct can also contain other elements that are used as configuration parameters
860  // for the device plugin.
861  // Example:
862  // non_ethercat_devices:
863  // netft1:
864  // type: NetFT
865  // address: 192.168.1.1
866  // publish_period: 0.01
867  // netft2:
868  // type: NetFT
869  // address: 192.168.1.2
870  // publish_period: 0.01
871  // dummy_device:
872  // type: Dummy
873  // msg: "This a dummy device"
874  //
875  // The name of the device is used to create a ros::NodeHandle that is
876  // that is passed to construct function of device class.
877  // NOTE: construct function is not the same as C++ construtor
878  if (!node_.hasParam("non_ethercat_devices"))
879  {
880  // It is perfectly fine if there is no list of non-ethercat devices
881  return;
882  }
883 
885  node_.getParam("non_ethercat_devices", devices);
886  if (devices.getType() != XmlRpc::XmlRpcValue::TypeStruct)
887  {
888  ROS_ERROR("Rosparam 'non_ethercat_devices' is not a struct type");
889  return;
890  }
891 
892  MyXmlRpcValue my_devices(devices);
893  typedef XmlRpc::XmlRpcValue::ValueStruct::value_type map_item_t ;
894  BOOST_FOREACH(map_item_t &item, my_devices.getMap())
895  {
896  const std::string &name(item.first);
897  XmlRpc::XmlRpcValue &device_info(item.second);
898 
899  if (device_info.getType() != XmlRpc::XmlRpcValue::TypeStruct)
900  {
901  ROS_ERROR("non_ethercat_devices/%s is not a struct type", name.c_str());
902  continue;
903  }
904 
905  if (!device_info.hasMember("type"))
906  {
907  ROS_ERROR("non_ethercat_devices/%s 'type' element", name.c_str());
908  continue;
909  }
910 
911  std::string type(static_cast<std::string>(device_info["type"]));
912 
914  configNonEthercatDevice(name,type);
915  if (new_device != NULL)
916  {
917  slaves_.push_back(new_device);
918  }
919  }
920 }
921 
922 
924 {
925  if (NULL == oob_com_)
926  return;
927 
928  { // Count number of devices
929  EC_Logic *logic = EC_Logic::instance();
930  unsigned char p[1];
931  EC_UINT length = sizeof(p);
932 
933  // Build read telegram, use slave position
934  APRD_Telegram status(logic->get_idx(), // Index
935  0, // Slave position on ethercat chain (auto increment address)
936  0, // ESC physical memory address (start address)
937  logic->get_wkc(), // Working counter
938  length, // Data Length,
939  p); // Buffer to put read result into
940 
941  // Put read telegram in ethercat/ethernet frame
942  EC_Ethernet_Frame frame(&status);
943  oob_com_->txandrx(&frame);
944 
945  // Worry about locking for single value?
946  diagnostics_.device_count_ = status.get_adp();
947  }
948 
949  for (unsigned i = 0; i < slaves_.size(); ++i)
950  {
952  d->collectDiagnostics(oob_com_);
953  }
954 }
955 
956 
957 // Prints (error) counter infomation of network interface driver
958 void EthercatHardware::printCounters(std::ostream &os)
959 {
960  const struct netif_counters &c(ni_->counters);
961  os << "netif counters :" << endl
962  << " sent = " << c.sent << endl
963  << " received = " << c.received << endl
964  << " collected = " << c.collected << endl
965  << " dropped = " << c.dropped << endl
966  << " tx_error = " << c.tx_error << endl
967  << " tx_net_down = " << c.tx_net_down << endl
968  << " tx_would_block= " << c.tx_would_block << endl
969  << " tx_no_bufs = " << c.tx_no_bufs << endl
970  << " tx_full = " << c.tx_full << endl
971  << " rx_runt_pkt = " << c.rx_runt_pkt << endl
972  << " rx_not_ecat = " << c.rx_not_ecat << endl
973  << " rx_other_eml = " << c.rx_other_eml << endl
974  << " rx_bad_index = " << c.rx_bad_index << endl
975  << " rx_bad_seqnum = " << c.rx_bad_seqnum << endl
976  << " rx_dup_seqnum = " << c.rx_dup_seqnum << endl
977  << " rx_dup_pkt = " << c.rx_dup_pkt << endl
978  << " rx_bad_order = " << c.rx_bad_order << endl
979  << " rx_late_pkt = " << c.rx_late_pkt << endl;
980 }
981 
982 
983 bool EthercatHardware::txandrx_PD(unsigned buffer_size, unsigned char* buffer, unsigned tries)
984 {
985  // Try multiple times to get proccess data to device
986  bool success = false;
987  for (unsigned i=0; i<tries && !success; ++i) {
988  // Try transmitting process data
989  success = em_->txandrx_PD(buffer_size_, this_buffer_);
990  if (!success) {
992  }
993  // Transmit new OOB data
994  oob_com_->tx();
995  }
996  return success;
997 }
998 
999 
1000 bool EthercatHardware::publishTrace(int position, const string &reason, unsigned level, unsigned delay)
1001 {
1002  if (position >= (int)slaves_.size())
1003  {
1004  ROS_WARN("Invalid device position %d. Use 0-%d, or -1.", position, int(slaves_.size())-1);
1005  return false;
1006  }
1007 
1008  if (level > 2)
1009  {
1010  ROS_WARN("Invalid level : %d. Using level=2 (ERROR).", level);
1011  level = 2;
1012  }
1013 
1014  string new_reason("Manually triggered : " + reason);
1015 
1016  bool retval = false;
1017  if (position < 0)
1018  {
1019  for (unsigned i=0; i<slaves_.size(); ++i)
1020  {
1021  if (slaves_[i]->publishTrace(new_reason,level,delay))
1022  {
1023  retval = true;
1024  }
1025  }
1026  }
1027  else
1028  {
1029  retval = slaves_[position]->publishTrace(new_reason,level,delay);
1030  if (!retval)
1031  {
1032  ROS_WARN("Device %d does not support publishing trace", position);
1033  }
1034  }
1035  return retval;
1036 }
vector< diagnostic_msgs::KeyValue > values_
unsigned reset_motors_service_count_
Number of times reset_motor service has been used.
static void timingInformation(diagnostic_updater::DiagnosticStatusWrapper &status, const string &key, const accumulator_set< double, stats< tag::max, tag::mean > > &acc, double max)
Helper function for converting timing for diagnostics.
d
static const unsigned dropped_packet_warning_hold_time_
Number of seconds since late dropped packet to keep warning.
#define ROS_FATAL(...)
EthercatHardwareDiagnostics diagnostics_
Diagnostics information use by publish function.
uint16_t length
Definition: wg_util.h:119
void publish(const boost::shared_ptr< M > &message) const
EtherCAT_Master * em_
void diagnosticsThreadFunc()
Publishing thread main loop.
void haltMotors(bool error, const char *reason)
void summary(unsigned char lvl, const std::string s)
ROSCONSOLE_DECL void initialize()
pr2_hardware_interface::HardwareInterface * hw_
void initialize(const std::string &interface)
void init(char *interface, bool allow_unprogrammed)
Initialize the EtherCAT Master Library.
MyXmlRpcValue(XmlRpc::XmlRpcValue &value)
XmlRpcServer s
std::vector< boost::shared_ptr< EthercatDevice > > slaves_
struct netif * ni_
accumulator_set< double, stats< tag::max, tag::mean > > pack_command_acc_
time taken by all devices packCommand functions
unsigned halt_motors_service_count_
Number of time halt_motor service call is used.
unsigned halt_motors_error_count_
Number of transitions into halt state due to device error.
void stop()
Stops publishing thread. May block.
ros::NodeHandle node_
bool motors_halted_
True if motors are halted.
bool publishTrace(int position, const string &reason, unsigned level, unsigned delay)
Ask one or all EtherCAT devices to publish (motor) traces.
unsigned char * prev_buffer_
Type const & getType() const
#define ROS_WARN(...)
EthernetInterfaceInfo ethernet_interface_info_
Information about Ethernet interface used for EtherCAT communication.
void addf(const std::string &key, const char *format,...)
accumulator_set< double, stats< tag::max, tag::mean > > txandrx_acc_
time taken by to transmit and recieve process data
const char * motors_halted_reason_
reason that motors first halted
void publishDiagnostics()
Collects raw diagnostics data and passes it to diagnostics_publisher.
void publishDiagnostics()
Publishes diagnostics.
unsigned timeout_
Timeout (in microseconds) to used for sending/recieving packets once in realtime mode.
vector< EthercatDevice * > devices
Definition: motorconf.cpp:65
void collectDiagnostics()
Collects diagnotics from all devices.
unsigned int buffer_size_
bool txandrx_PD(unsigned buffer_size, unsigned char *buffer, unsigned tries)
Send process data.
uint16_t status
void update(bool reset, bool halt)
Send most recent motor commands and retrieve updates. This command must be run at a sufficient rate o...
boost::shared_ptr< EthercatDevice > configSlave(EtherCAT_SlaveHandler *sh)
#define ROS_INFO(...)
std::vector< boost::shared_ptr< EthercatDevice > > slaves_
uint64_t last_dropped_packet_count_
Count of dropped packets last diagnostics cycle.
unsigned char * buffers_
diagnostic_updater::DiagnosticStatusWrapper status_
unsigned int num_ethercat_devices_
EthercatHardwareDiagnosticsPublisher(ros::NodeHandle &node)
boost::shared_ptr< EthercatDevice > configNonEthercatDevice(const std::string &product_id, const std::string &data)
realtime_tools::RealtimePublisher< std_msgs::Bool > motor_publisher_
boost::condition_variable diagnostics_cond_
void initialize(const string &interface, unsigned int buffer_size, const std::vector< boost::shared_ptr< EthercatDevice > > &slaves, unsigned int num_ethercat_devices_, unsigned timeout, unsigned max_pd_retries)
Initializes hardware publish.
static void changeState(EtherCAT_SlaveHandler *sh, EC_State new_state)
EthercatOobCom * oob_com_
void mergeSummary(unsigned char lvl, const std::string s)
unsigned char * this_buffer_
bool hasMember(const std::string &name) const
struct netif_counters counters_
diagnostic_msgs::DiagnosticArray diagnostic_array_
static void updateAccMax(double &max, const accumulator_set< double, stats< tag::max, tag::mean > > &acc)
EthercatHardwareDiagnosticsPublisher diagnostics_publisher_
void publish(const unsigned char *buffer, const EthercatHardwareDiagnostics &diagnostics)
Triggers publishing of new diagnostics data.
~EthercatHardware()
Destructor.
unsigned timeout_
Timeout controls how long EtherCAT driver waits for packet before declaring it as dropped...
EthercatHardware(const std::string &name)
Constructor.
bool getParam(const std::string &key, std::string &s) const
unsigned max_pd_retries_
Number of times (in a row) to retry sending process data (realtime data) before halting motors...
static Time now()
void mergeSummaryf(unsigned char lvl, const char *format,...)
void add(const std::string &key, const T &val)
boost::mutex diagnostics_mutex_
mutex protects all class data and cond variable
bool hasParam(const std::string &key) const
void publishDiagnostics(diagnostic_updater::DiagnosticStatusWrapper &d)
Collect and append ethernet interface diagnostics.
accumulator_set< double, stats< tag::max, tag::mean > > unpack_state_acc_
time taken by all devices updateState functions
ROSCPP_DECL int close_socket(socket_fd_t &socket)
void printCounters(std::ostream &os=std::cout)
static double min(double a, double b)
Definition: motor_model.cpp:4
unsigned max_pd_retries_
Max number of times to retry sending process data before halting motors.
#define ROS_ERROR(...)
bool halt_after_reset_
True if motor halt soon after motor reset.
ros::Time last_dropped_packet_time_
Time last packet was dropped 0 otherwise. Used for warning about dropped packets. ...
static const bool collect_extra_timing_
XmlRpcValue::ValueStruct & getMap()
accumulator_set< double, stats< tag::max, tag::mean > > publish_acc_
time taken by any publishing step in main loop


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