pr2 ethercat should report last time halt occurred in diagnostics (ros ticket #4332)

[ahendrix]

I will add a diagnostic key-value pair in the Ethercat Master diagnostics that contains that time that a motor halt last occurred.

trac data:

• Owner: dking
• Reporter: blaise
• Reported at: Fri May 14 16:31:41 2010
• URL: https://code.ros.org/trac/wg-ros-pkg/ticket/4332

[ahendrix]

[blaise] Actually, this might be a rob ticket...

[ahendrix]

[blaise] Just curious why the won't fix? It is still something that strikes me as useful, and that we'll wish we had next time we run into one of these scheduling/motors halting problems.

[ahendrix]

[dking] Patch for pr2_ethercat_drivers 1.5

{{{

--- include/ethercat_hardware/ethercat_hardware.h (revision 48057) +++ include/ethercat_hardware/ethercat_hardware.h (working copy) @@ -84,6 +84,7 @@ struct netifcounters counters; bool input_thread_isstopped; bool motorshalted; //!< True if motors are halted

• ros::Time last_time_motorshalted; //!< Last time motors halted

  static const bool collect_extratiming = true; };

  Index: src/ethercat_hardware.cpp

  --- src/ethercat_hardware.cpp (revision 48057) +++ src/ethercat_hardware.cpp (working copy) @@ -53,6 +53,7 @@ halt_motors_errorcount(0) { resetMaxTiming();

• last_time_motorshalted = ros::Time::now(); // Assume motors start in halted state }

  void EthercatHardwareDiagnostics::resetMaxTiming() @@ -374,7 +375,7 @@ boost::unique_lockboost::mutex lock(diagnosticsmutex, try_lock); if (lock.owns_lock()) {

• // Make copies of diagnostic data for dianostic thread

• // Make copies of diagnostic data for diagnostic thread memcpy(diagnosticsbuffer, buffer, buffersize); diagnostics_ = diagnostics; // Trigger diagnostics publish thread @@ -446,6 +447,8 @@ }

  status.add("Motors halted", diagnostics.motorshalted ? "true" : "false");

• status.addf("Last time motors halted", "%f", diagnostics.last_time_motorshalted.toSec());
• status_.addf("EtherCAT devices (expected)", "%d", numslaves); status.addf("EtherCAT devices (current)", "%d", diagnostics.devicecount); ethernet_interfaceinfo.publishDiagnostics(status_); @@ -641,12 +644,16 @@ motorpublisher.unlockAndPublish(); }
• if ((haltmotors && !old_halt_motors) && (halt_motors_error))
• if (haltmotors && !old_halt_motors) {
• ++diagnostics_.halt_motors_errorcount;
• if ((update_start_time - lastreset) < ros::Duration(0.5))
• diagnostics_.last_time_motorshalted = update_start_time;
• if (halt_motors_error) {
• diagnostics_.halt_afterreset = true;
• ++diagnostics_.halt_motors_errorcount;
• if ((update_start_time - lastreset) < ros::Duration(0.5))
• {
• diagnostics_.halt_afterreset = true;
• } } }

dking@bli:/wg/stor1a/dking/dros/pr2_ethercat_drivers/ethercat_hardware$ svn di

Index: include/ethercat_hardware/ethercat_hardware.h

--- include/ethercat_hardware/ethercat_hardware.h (revision 48057) +++ include/ethercat_hardware/ethercat_hardware.h (working copy) @@ -84,6 +84,7 @@ struct netifcounters counters; bool input_thread_isstopped; bool motorshalted; //!< True if motors are halted

• ros::Time last_time_motorshalted; //!< Last time motors halted

  static const bool collect_extratiming = true; };

  Index: src/ethercat_hardware.cpp

  --- src/ethercat_hardware.cpp (revision 48057) +++ src/ethercat_hardware.cpp (working copy) @@ -53,6 +53,7 @@ halt_motors_errorcount(0) { resetMaxTiming();

• last_time_motorshalted = ros::Time::now(); // Assume motors start in halted state }

  void EthercatHardwareDiagnostics::resetMaxTiming() @@ -374,7 +375,7 @@ boost::unique_lockboost::mutex lock(diagnosticsmutex, try_lock); if (lock.owns_lock()) {

• // Make copies of diagnostic data for dianostic thread

• // Make copies of diagnostic data for diagnostic thread memcpy(diagnosticsbuffer, buffer, buffersize); diagnostics_ = diagnostics; // Trigger diagnostics publish thread @@ -446,6 +447,8 @@ }

  status.add("Motors halted", diagnostics.motorshalted ? "true" : "false");

• status.addf("Last time motors halted", "%f", diagnostics.last_time_motorshalted.toSec());
• status_.addf("EtherCAT devices (expected)", "%d", numslaves); status.addf("EtherCAT devices (current)", "%d", diagnostics.devicecount); ethernet_interfaceinfo.publishDiagnostics(status_); @@ -641,12 +644,16 @@ motorpublisher.unlockAndPublish(); }
• if ((haltmotors && !old_halt_motors) && (halt_motors_error))
• if (haltmotors && !old_halt_motors) {
• ++diagnostics_.halt_motors_errorcount;
• if ((update_start_time - lastreset) < ros::Duration(0.5))
• diagnostics_.last_time_motorshalted = update_start_time;
• if (halt_motors_error) {
• diagnostics_.halt_afterreset = true;
• ++diagnostics_.halt_motors_errorcount;
• if ((update_start_time - lastreset) < ros::Duration(0.5))
• {
• diagnostics_.halt_afterreset = true;
• } } }
  }}}